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ABSTRACT 



Underwater walking machines offer a potential for replacement of human divers in 
certain aspects of underwater construction and inspection One such vehicle, Aquarobot, 
is currently under test in Japan However, this vehicle is currently too slow to be 
economically utilized, and limited hardware availability restricts progress in control 
software improvements. A software dynamic simulation model is desirable to relieve this 
restricted access. The problem addressed by this research is the modeling of system 
dynamics of underwater walking vehicles with sufficient simplification to achieve a 
real-time simulation. The approach taken includes an object-oriented, massless leg robot 
dynamic model and employs a high performance graphics rendering toolkit. 

The resulting simulations of a robotic joint actuator and of the robot itself, utilizing 
springs and dampers in the joints, runs in real-time. The robot simulation model executes 
on a four-processor machine with under fifteen percent utilization of the processor 
dedicated to system dynamics. This result indicates that the simulation is likely to retain 
real-time capability after replacing the springs and dampers with the more accurate joint 
actuator model also developed in this thesis. 
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I. INTRODUCTION 



A. AQUAROBOT 

Aquarobot is a six-legged "insect type", articulated, experimental prototype robot 
under development at the Port and Harbour Research Institute (PHRI) in Japan This 
robot is being investigated as an alternative to the currently used human divers for seawall 
construction and inspection. While the divers are fully capable, the limited stay time at 
required depths make progress slow and expensive. A walking robot is preferred to 
tracked, wheeled, or floating versions for its abilities to maneuver without disturbing the 
bottom enough to cloud the water and restrict visibility, and to provide a stable reference 
platform from which measurements can be made [Iwasaki, 1987], The prototype 
Aquarobot has successfully walked underwater and demonstrated functional feasibility for 
the intended task, but it is currently too slow to be economically utilized [Davidson, 
1993], The Naval Postgraduate School (NPS) is working with PHRI to upgrade 
Aquarobot's control software, from the original version written in BASIC, to improve its 
operating speed. 

B. GOALS 

The goal of this thesis is to investigate the feasibility of dynamic modeling of 
Aquarobot with sufficient simplifications to achieve a real-time simulation. The simulation 
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model should be statically accurate and dynamically approximate. The major 
simplifications considered are: 

(1) massless legs, 

(2) body mass evenly distributed in a cylinder, 

(3) center of mass at geometric center of the inboard leg joints, 

(4) infinite friction for foot contact with surface, 

The dynamic Aquarobot model of this thesis uses springs and dampers in place of 
joint actuators for this initial feasibility study. A joint actuator simulation model, including 
servomotor and controller models, is also developed and is intended to eventually replace 
the springs and dampers. Inputs to that model will be control software orders to the joint 
motor controller. 

C. ORGANIZATION 

Chapter II of this thesis reviews previous and concurrent work in the area of walking 
robots with emphasis on work related to Aquarobot. Chapter III provides a more detailed 
description of Aquarobot and introduces the software tools used. Chapters IV and V 
develop the necessary mathematical models and then present prototype dynamic 
simulation models for an Aquarobot joint actuator and for Aquarobot itself (with springs 
and dampers in place of joint actuators). Chapter VI reviews the results of simulations 
accomplished with the models introduced in Chapters IV and V. Finally, Chapter VII 
presents some conclusions, suggestions for further research, and a summary. 
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II. SURVEY OF PREVIOUS AND CONCURRENT WORK 



A. INTRODUCTION 

To place Aquarobot research in relative perspective, this chapter begins with a brief 
historical review of walking machines An overview of ongoing Aquarobot research at 
NPS follows and places this thesis in context Other contributions, some completed and 
some currently in progress, are described. Also, as this thesis is a continuation of previous 
work, a short review of some of the key elements of that work is presented to provide a 
starting frame of reference. 

B. BRIEF HISTORY OF WALKING MACHINES 

In an early exploration of walking mechanisms, 1965 to 1968, General Electric 
Corporation built a four legged vehicle called the Quadruped Transporter Because of the 
lack of theory and technology, designers incorporated human sensing and neural control of 
the limbs by attaching "position following, force feedback" control levers to the operator's 
arms and legs. Each of these levers had three degrees of freedom, corresponding to those 
of the leg it controlled. Very few mastered the skills required to operate the vehicle, and 
those that did found it to be very demanding [McGhee, 1985], While the Quadruped 
Transporter lacked practicality, its successful implementation encouraged further research. 



3 



Automation of low level tasks, such as individual limb control, leaves the operator 
free to concentrate on higher level, "supervisory control" of the vehicle. In 1977, this 
method of control was used in the Ohio State University (OSU) "Hexapod Vehicle." The 
operator controlled vehicle speed and direction, using a joystick, while limb motion 
control and coordination was handled by computer. This machine was utilized in the 
development of gait algorithms [McGhee, 1985], 

[McGhee, 1986] addresses the energy-efficiency issue of limb control and introduces 
a method used to achieve a cyclic leg motion without requiring the reversal of drive 
motors This approach was demonstrated in MELCRABs 1 and 2 at Mechanical 
Engineering Laboratory in Japan. 

From 1981 to 1986, the Adaptive Suspension Vehicle (ASV) was constructed and 
tested at OSU. The ASV was a six-legged vehicle designed for outdoor operation on 
irregular, unmapped terrain and included a self contained, onboard power supply. It 
carried an operator who exercised supervisory control via a joystick and keypad. Leg 
coordination and foothold selection were fully automated; the latter was allowed by 
employment of extensive environmental sensors including an optical terrain scanner 
[Waldron, 1986], Related later work [Kwak, 1990] explores the use of rule-based limb 
motion coordination to implement a "free," non-periodic, gait permitting on-line 
optimization of foot placement. 
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C. AQUAROBOT RESEARCH AT NPS 

Aquarobot research at NPS is divided into two concurrent phases: control and 
simulation. The first of these, control, consists of Aquarobot control software 
development The second phase, simulation, involves development of a graphical 
computer simulation of the Aquarobot hardware The simulator is required for the final 
stages of the control software development, including testing. 

1. Control Software Development 

While final development and testing of control software depends the availability 
of the simulation model, some work has been accomplished prior to such availability In 
[Schue, 1993] an algorithm is presented for statically stable, alternating tripod gait 
planning and foot path planning for smooth leg motion during walking on flat terrain. 
Further developments in gait planning algorithms and demonstration of alternative gaits, 
which allow variable direction and speed but require continuous adjustment of leg liftoff 
and touchdown sequence, are reviewed in [Yoneda, 1993], 

2. Simulation Software Development 

The framework for the Aquarobot simulation model is provided in [Davidson, 
1993], in which an object-oriented kinematic model is developed. Both 
Danevit-Hartenberg (DH) and Craig (Modified Danevit-Hartenberg or MDH) methods are 
presented and then compared. The fundamental difference in the two methods is in the 
coordinate systems used for a "link," the rigid limb component between two joints The 
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DH methodology utilizes the outboard, closer to end-effector, joint as the coordinate 
system origin while the Craig method uses the inboard, closer to body, joint. The Craig 
version of Davidson's kinematic model is used in this thesis. 

[Goetz, 1994] explores a variety of enhancements for the Aquarobot simulation 
model. A graphics model, which incorporates a surrounding operating environment 
(terrain), is developed to replace the original stick figure. The model includes I/O control 
interfaces (i.e. keyboard, joystick, spaceball) and foot/ground collision detection. 

A complete, unsimplified physical dynamic simulation model of Aquarobot, 
including the hydrodynamic forces of its operating environment, is also being developed 
[McMillan, 1993]. While this simulation is not expected to run in real time, it will provide 
valuable data for comparison to the simplified model. 

D. REUSED SOFTWARE 

As mentioned above, the Aquarobot simulation presented in this thesis is based on the 
model described by [Davidson, 1993], A summary of the key features of that model is 
provided here for quick reference. 

1. Rigid Body Class 

In both LISP and C++ versions of the Aquarobot model, system dynamics for six 
degrees of freedom, three translational and three rotational, are handled within a "rigid 
body class" from [Davidson, 1993], System state variables include world coordinate 
position and orientation, stored in a 4x4 "body to world" coordinate transformation 



6 



matrix, called the "H-matrix", and velocities in body coordinates Euler integrations are 
used for dynamic updates Acceleration equations in body coordinates are 



U = vr-wq + ^-g sine , 


(2 1) 


v = \ip- ur + ^ J rg cos0 sin(j) , 


(2 2) 


w = uq - vp + ^ +g cos0 cos<{) , 


(2.3) 
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where m is body mass; g is gravitational acceleration, in world coordinates; / and /„ 
are the moments of inertia;/= (f x ,f ,f z ) is the vector of applied forces; T = ( L , M, N) is the 
vector of applied torques; theta and phi are Euler pitch and roll angles, respectively; u, v, 
w are the components of translational velocity; and p, q, r are rotational rate components 
[Frank, 1969], With a single exception, g, the above values are expressed in body 
coordinates. The dynamic update is achieved by determining incremental position and 
orientation changes, in body coordinates, and using those to generate an incremental 
motion matrix which is then post-multiplied with the body's H-matrix, and using that result 
to update (replace) the H-matrix. Euler integrations and equations 2.1 through 2.6 are 
used to update the velocity state variables [Davidson, 1993], 
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2. Kinematic Model 



Each Aquarobot limb (leg) is made up of a series of links: the inboard end of the 
series being physically connected to the robot's body and the outboard end the foot. Using 
the Craig method, a coordinate system at the outboard end of a link is described relative to 
the coordinate system at the inboard end by a transformation matrix called a "T-matrix". 
The T-matrix depends on the physical construction of the link and, in the case of rotary 
links, the rotation angle of the inboard joint. The origin of each such coordinate system is 
the position of the joint between two such links, and the z-axis is aligned so that a joint 
rotation is a z-axis rotation. The entire system is kept in a hierarchical structure of "rigid 
bodies", with H-matrix updates done from the top down: i.e. a link's H- matrix may be 
updated only after the link next inboard is updated (the robot body in the case of the 
inboard end of each leg) [Davidson, 1993], 

[HJ " [H,,] [T,] (2.7) 

E. SUMMARY 

The development of the Aquarobot simulation model will directly support final 
development and testing of Aquarobot control software. The dynamic model developed in 
this thesis is based on a previously developed kinematic model. Before describing the 
dynamic model, a more detailed description of Aquarobot and an overview of the software 
tools used is needed and is provided in Chapter HI. 
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III. DESCRIPTION OF AQUAROBO T VEHICLE AND 
SIMULATION E N VI RONM E NT 



A. INTRODUCTION 

This chapter provides a physical description of Aquarobot, including some details that 
are beyond the scope of models developed in this thesis. Special emphasis is given to the 
joint actuators as they are the primary feature to be modeled. In addition, the software 
tools used in the development are introduced. 

B. PHYSICAL DESCRIPTION OF AQUAROBOT 

Figure 3.1 is a photograph of Aquarobot which has a body, that is generally 
cylindrical in shape, and six legs, equally spaced at sixty degrees apart Mounted on top of 
the body is a camera boom with three rotary joints for positioning. The boom is equipped 
with an ultrasonic ranging device to assist in the scaling of measurements. Within the 
body are two inclinometers (for attitude sensing), a gyrocompass, and a depth sensing 
device. Aquarobot is computer controlled from the surface via a four centimeter diameter 
tether cable attached to the top of the body. The cable carries control signals to the robot 
and returns sensor signals back to the controlling computer. 

Each of the six identical insect type legs has three rotary joints, which are driven by 
the joint actuators described in the next section, and a freely rotating ball joint to attach 
the disc shaped foot. Each foot has a pressure sensitive touch sensor to provide an 
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indication of ground contact. Figure 3.2 illustrates the axis of rotation for each joint in an 
Aquarobot leg. 




Figure 3.1 

Photograph of Aquarobot 




Figure 3.2 

Aquarobot Leg Construction 
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C. AQUAROBOT JOINT ACTUATORS 

Figure 3.3 is a block diagram of an Aquarobot joint actuator Control software sends 
incremental rotation orders to the controller as pulses, with polarity indicating the desired 
direction of rotation. Motor response is fed back to the control software as pulses in the 
same manner. These feedback pulses are produced by a pulse generator on the motor 
shaft. Additional signals to and front the control software include pulse counter (PC) 
overflow', pulse counter clear, and driv er enable. 




Figure 3.3 

Aquarobot Joint Actuator 



The difference between the actual and ordered position, the error signal of the motor, 
is kept in the pulse counter. Orders increment the counter, plus or minus, depending on 
desired direction; response pulses decrement it. One hundred pulses are required for one 
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motor revolution (determined by the motor shaft pulse generator output). The counter 
overflows if the maximum count, +/- 6144 pulses, is exceeded. 

A digital to analog converter (D/A) produces a DC signal directly proportional to the 
current count in the pulse counter, and its output is the motor driver's primary input. That 
is. the larger the error, the higher the voltage applied to the motor to correct the error. A 
maximum count of +/-6144 is converted to +/- 10VDC, i.e. 

DA out — count * ( 1 0/6144). (3.1) 

A high gain for the error signal is desirable for fast response and for sufficient 
response to small errors. However, by itself the high gain would cause severe overshoot 
and oscillations in the motor. Degenerative feedback, proportional to motor speed, is 
used to prevent, or at least minimize, this overshoot. This degenerative feedback is 
provided by the frequency to voltage converter (F/V) which monitors the pulse generator 
output. The output is 3VDC per thousand RPM's, so 

FV ou i = RPM * (3/1000) (3.2) 

The driver amplifies outputs from the D/A and F/V converters to produce the source 
voltage ( F ) for the servomotor. So far, we have 

V s = G x DA oul -G 2 FV out , (3.3) 

where G, and G 2 are respective gains. However, the driver is actually more complex and 
includes an additional internal feedback path. 

The signals form the D/A and F/V converters are amplified and fed into the pulse 
width modulator (PWM) with the F/V signal inserted between inverting amplifiers to 
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provide degeneration. A current pickup on the PWM output line provides a signal 
proportional the current drawn by the motor, and therefore proportional to the motor 
torque. Recall that for a given voltage applied to a motor, it has a limited speed due to 
back EMF. As the motor approaches that speed, it draws less current. This current 
(torque) feedback signal provides regenerative feedback in the driver during motor 
acceleration, thus reducing the response time. Adding the torque feedback to Equation 
3.3 yields 

!\ = G\DA ou , - G:Fl +Gi/„. (3.4) 

where l a represents the motor's armature current. Actually, this equation still neglects 
some complexities in the Aquarobot joint controller. Not shown in Figure 3.3 are 
feedback capacitors around amplifiers which further modify the equation for E, These 
effects are not modeled in the work of this thesis. 

The motor is driven by a squarewave rather than a DC voltage. Figure 3.4 illustrates 
idealized signals. The function of the PWM is to provide a squarewave of constant 
amplitude, zero to +/- 75 volts, with a duty cycle proportional to the input signal, so 1 in 
Equation 3.4 is actually an average value. Motor response to this signal is very close to its 
response to a DC voltage equal to the average voltage of the squarewave. Applying 
Signals 1 or 2 of Figure 3.4 to the motor is thus equivalent to applying -t-25VDC or 
-25VDC. respectively. This methodology is used primarily for its efficiency advantage 
over a DC linear amplifier [Truxal. 1958], 
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Figure 3.4 

PWM Outputs For 33 °o Duty Cycles 



Attached to each servomotor is a harmonic reduction gear, built together as a single 
unit. The model used for joint one in each leg, RA-20, has a reduction ratio of 161:1, 
while that used for joints two and three, RH-25, has a 160: 1 ratio. In addition, joints two 
and three have a bevel gears cascaded with the harmonic gears with 3:1 and 2:1 ratios, 
respectively. Figure 3.5 summarizes the total reduction ratios and gives the control pulses 
required for a single joint shaft revolution. 



Joint 


1 


2 




Harmonic Gear 


161:1 


160:1 


160:1 


Bevel Gear 


NA 


3:1 


2:1 


Total Gear Ratio 


161:1 


480:1 


320: 1 


Pulses / Revolution 


16,100 


48,000 


32,000 



Figure 3.5 

Total Reduction Gear Ratios and 
Control Pulse Requirement per Joint Revolution 
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D. SOFTWARE TOOLS 



C++ is the programming language selected for modeling Aquarobot. This choice was 
based on both hardware and software considerations. IRIS Performer, a C visual 
simulation toolkit, provides rendering that is fast enough to display a real time dynamic 
simulation [Goetz. 1994], Common LISP was used for rapid prototyping and testing 
during the early stages of development. 

1. C++ 

In a long term project with many contributors, object oriented design provides a 
high lev el of abstraction which promotes rapid system lev el comprehension by new team 
members as well as loosely coupled, easily maintained source code. Not only does C++ 
support the object oriented design paradigm, but also, it is based on. and includes as a 
subset, the highly efficient, structured language. C [Wiener, 1988], 

The simulation platform, both here and at PHRI. is an IRIS Reality Engine. The 
C/C++ compilers delivered with the IRIS systems are very efficient, due to direct access to 
the low level hardware, and are intended to be used as native development languages. 

Popularity and widespread use make C/C++ source code portable, and while 
portability is not a key issue for the Aquarobot simulation, it is very much so for the 
Aquarobot control software which is likely to have to survive hardware upgrades. 
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2. Performer 



IRIS Performer is used primarily for its performance as a rendering tool. 
Performer is a hardware oriented, C/C++ graphics tool kit designed to operate on Silicon 
Graphics products. Its routines take full advantage of the "known hardware" platform to 
allow much higher performance than routines written for generic hardware. Also, some 
precision is traded for speed as real numbers in Performer data structures are single 
precision "floats" rather than "doubles". Upon initialization. Performer detects hardware 
capability and automatically sets up a multiprocessing environment with shared memory 
when running on a multiprocessor machine. The default configuration, which the user 
may override, is separate processors assigned for (1) user application, (2) graphics 
database culling, and (3) drawing the culled database to a graphics window [SGI, 1992], 
Performer's graphics database is stored in a hierarchical data structure, a tree. 
The structure is culled and rendered by doing a pre-order traversal with child nodes 
inheriting the accumulated environments (transformations) of all ancestors traversed in the 
current path. These accumulated environments are kept on a stack and are popped off 
when traversing back up the tree. The basic nodes are Scene (the root). Static Coordinate 
System (SCS), Dynamic Coordinate System (DCS) (variable for motion where an SCS is 
fixed), Group (a container for children requiring a common transformation), and Geode (a 
container for polygons to be rendered). Geodes must be leaves but may have more than 
one parent. This reduces the memory requirements when a group of polygons is to be 
rendered more than once in a frame (two or more identical objects) [SGI, 1992], 
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Robotics and mechanics users must be aware that graphics standards are used in 
matrix construction and operations. The graphics matrix is the transpose of the standard 
robotics matrix [Fu, 1987], GA/ ; = RM n . This is only important when using the pfMatrix 
data type and operations outside those that directly support the graphics database, or 
when directly manipulating individual elements, rows, and / or columns. Since MxS 
(jV'xA/ 1 ) 1 , the order of matrix multiplications may also hav e to be rev ersed. Lastly . X and 
Y axis rotations are as expected; however in Performer, pitch refers to an X. rather than 
Y, axis rotation, and likewise, roll refers to a Y axis rotations [SGI, 1992]. 

Synchronization for a real-time application may be achieved by setting 
Performer's "desired frame rate" and then using the pfSvnc function call. TTie pfSync 
function will put all processes to sleep between each frame to force the desired frame rate. 
If the desired frame rate is faster than can be achieved. pfSync will have no effect, and the 
application will free run at its fastest speed. In the Aquarobot simulation, the frame rate is 
set to tw enty frames per second and then a fixed delta-time of one twentieth of a second is 
used for dynamic updates. Reading the internal clock for delta-time would have been 
equivalent providing the application is capable of running at the desired frame rate. The 
fixed delta-time ensures control over data points [SGI, 1992], 

3. LISP 

The problem with "rapid, experimental" prototyping in C/C+-*-, as well as other 
compiled languages, is the difficulty with testing and verification. Specifically, a test shell 
must be written, compiled and run to thoroughly test a block of source code. If the test 
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results in the detection of errors, debugging tools are available, but the source code must 
be recompiled to include them. Other methods, such as insertion of additional lines of 
code, are also available but also require multiple compilations. In any case, logic errors 
are still difficult to find when they exist in large blocks of code, especially when they only 
apply to specific inputs. LISP, on the other hand, is an ideal language for experimental 
work. LISP is an interpreted, functional language which gives the developer the ability to 
call any defined function directly from the command prompt. The function's return value 
is immediately displayed for easy comparison to expected values; test routines are not 
required. Since functions may be nested, these test calls may include any desired level of 
abstraction. Variables are also directly accessible in LISP and may be inspected at any 
time. The basic structure of a LISP program is an on-line database of definitions: 
constants, functions, and symbols (pointers to variables) [Koschmann, 1990], 

Weak typing in LISP allows additional flexibility. Typing is done dynamically at 
run time. For example, a single definition of the function " minim um" may be used for 
integers, reals, or any argument type for which the operator "<" is defined. Variables, 
arguments, and return values may also be lists, allowing the developer to call a function 
with a comprehensive set of test inputs [Koschmann, 1990], 

The Common LISP Object System, CLOS, provides full support for object 
oriented programming. In this thesis, a prototype design using CLOS is first developed to 
implement system design decisions. Subsequent translation to C++ is then accomplished 
without abstract structure modifications. 
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E. SUMMARY 



This chapter physically describes Aquarobot with the intention of providing sufficient 
orientation prior to the model presentations. Several Aquarobot features are not included 
in the dynamic simulation model presented later in Chapter V The kinematic model 
includes the body, legs and feet but not the camera boom. Joint position, foot contact, 
and azimuth information are available; however, attitude and depth are not The tether 
cable and hydrodynamic forces, currents and viscosity, are also neglected. 

Also in preparation for the model presentations, the software tools utilized, along 
with the reasons for their selections, are introduced. Object-oriented system design and 
the need for a high performance made C++ and IRIS Performer ideal software tools for 
the Aquarobot simulation model. CLOS. with its capability for rapid testing of complex 
functions, was used for prototyping and model verification. 

The following chapters present dynamic simulation models of an Aquarobot joint 
actuator and Aquarobot itself. 
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IV. JOINT ACTUATOR MODEL 



A. INTRODUCTION 

The purpose of this chapter is to present a simulation model for a single Aquarobot 
joint actuator. It begins with a review of the basic mathematics required to model 
serv omotors and reduction gears and then develops the joint actuator model. 

B. BASIC DC MOTOR 

A motor is a device used to convert electrical energy into mechanical energy. The 
force, F , on a current, /, carrying conductor of length / in a uniform magnetic field, B , is 
given by [Halliday, 1981] 

F = \idl x B. (4.1) 

If the conductor is fixed on a shaft, parallel to the shaft, at radius r, the resulting torque, 
tau, is given by [Halliday, 1981] 

t = r x F. (4.2) 

The motor’s armature includes a set of such conductors and is usually constructed so as to 
be symmetrical with respect to the axis of rotation; therefore, the total conductor length 
and relative position (to the magnetic field) is independent of the armature's angular 
position. Furthermore, if a constant magnetic field (i.e. permanent magnet or constant 
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current electromagnet) is used, then the force and resulting torque become directly 
proportional to the armature current, simplifying the above equations to 

F=iC, (4.3) 

and 

x,/ = /A' f . (4.4) 

where ton j is developed torque, and torque constant K, is a characteristic of the motor 
Hie armature current depends on applied voltage, armature resistance, and armature 
angular velocity, omega. The velocity dependency is due to the voltage induced in the 
conductors as they move through the field (Faraday’s Law). Since this induced voltage is 
of such polarity that it causes a decelerating torque (Lenz's Law), it is called Back or 
CounterEMF. l' h [Halliday, 1981] [McPherson, 1981] 

l’b = K b t o, (4.5) 

where K h is the motor's back EMF constant. 

Armature inductance is usually negligible for high quality servomotors, so using Ohm's 
Law’, the armature current, I a , is given by [Halliday, 1981] [McPherson, 1981] 




where V a is the voltage applied to the armature and R a is armature resistance. 

Combining Equations 4.4 through 4.6, the motor's developed torque is 

_ j iv" K t (V a -K b (ti) ( a n\ 

T J — I a&t — ^ w- ) 

Given no external torques and ignoring losses for now, for any V a there is an associated 
maximum speed, when V a = K h omega , that results in tau d = 0 and therefore no further 



21 



acceleration. Still ignoring losses, motor acceleration, omega-dot, is given by the standard 
rotational dynamics equation [Hallidav. 1981] 



CO = 



torque _ ij+Xj 
inertia fm+Jx 



(4.8) 



where J m is internal motor inertia, and tau x and J x are external torque and inertia, 
respectively. There are several sources for losses in a motor, but as long as the motor is 
operated within its prescribed limits, most of them may be consolidated into two groups: 
constant. F t . and directly proportional to velocity, F v omega. Some examples are friction, 
copper (rR). and windage [McPherson, 1981], External loads may also include constant 
and velocity dependent losses. The actual loss is state dependent and requires some 
special handling: 

(1) shaft turning (omega <> 0) : loss opposes omega 



State 2a may be handled by the method used for state 1, but since it is already detected by 
the test for state 2b, there is no reason to perform the arithmetic. Incorporating losses 
into Equation 4.8 yields 



|/o.s.s| = F c + F V |co| ; 

(2) shaft at rest (omega = 0) : loss opposes torque 



(4.9) 



(a) torque sufficient to overcome friction (|torque| > Fc) 
loss = F c ; 



(4.10) 



(b) torque insufficient to overcome friction (requires torque - loss = 0) 



loss = torque . 



(4.11) 




(4.12) 
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The only other significant loss not fitting into one of the groups above is the voltage drop 
across the motor brushes, brush drop loss 1 \ d . The voltage applied to the motor armature. 
T. is the motor source voltage. /' minus this brush drop loss [McPherson. 1981], 




(4.13) 



Combining Equations 4 7, 4.12 and 4.13. motor response depends on construction, state, 
applied voltage, and load (external torque and inertia). 



As a final note, motors are given voltage and load ratings. These are determined by 
motor construction and are intended to keep armature current within safe operating limits 
[McPherson. 1981], 



A reduction gear is a mechanical coupling device that provides a mechanical 
advantage allowing a higher speed, lower torque source to drive a lower speed, heavier 
load. This enables both source and load to operate at or near their optimal, most efficient 
speeds, even though those speeds are not the same. The gear ratio. //, is the ratio of the 
input and output shaft angular displacements, velocities and accelerations, theta, omega 
and omega-dot , respectively [Chen, 1993]. 




T x - Losses 



(4 14) 



C. REDUC TION GEAR 




(4.15) 




(4.16) 




(4.17) 
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While the output shaft speed is reduced by a factor of //. the torque is increased by a factor 
of// [Chen. 1993], 

T out — // T //i (4.18) 

Recalling and rearranging Equation 4.8 and applying it to the output shaft 

Jom = (4.19) 

U>out 



Substituting for omega-dot and tau oul , using Equations 4. 17 and 4. 18 



d oul — 



(<u„| ///) 



-> T. r 

= //V 



= // : J„ 



(4.20) 

(4.21) 

(4.22) 



This gives a coupling factor for inertia of n 2 , i.e. 



~ = (4.23) 

rn 

Combining a motor and a reduction gear as a drive train for some load. Equations 4. 12, 
4. 18 and 4.22, yields: 

(4.24) 



T,/ + (tj ,/n) - Losses 

co m 7 ~ i or 



J m + 



{jM) 



or 



co* = 



(/; t^) + t* - (n Losses ) 



(4.25) 



{jm n 2 ) +j, 

where m is the input (motor) side of the reduction gear, and x is the output side. 
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I). JOINT ACTUATOR SIMl LATION 



Figure 4 1 is a block diagram of the Joint Actuator Simulation Model Refer to 
Figure 3.3 for a comparison to the actual Aquarobot Joint Actuator. Since the model was 
in essence a "bench top" device, physical limitations, such as pulse counter overflow and 
motion limits, were not detected or enforced. Also, input pulses were replaced b\ a 
change in desired angular position of the joint shaft in revolutions, delta-theta, and the 
motor driver's rectangular wave output is simplified to its average value. Loading and 
operating instructions and a complete source code listing for this model may be found in 
Appendix A. 




The D/A converter is not necessary in the simulation model as the Pulse Counter 
stores the difference between the current and ordered shaft positions rather than a discrete 
pulse count. Motor velocity and armature current, slots values in the motor class, are 
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directly accessed to provide those inputs to the driver. The cascaded amplifiers in the 
driver are separated and then summed in order to simplify gain adjustments. Positional 
feedback is taken after the reduction gear to match the implementation of the positioning 
orders input. This is in contrast to the physical device in which feedback pulses come 
directly from the motor itself. 

1. Base Classes 

Several base classes are used in the joint actuator model. Refer to Appendix A 
for a source code listing. The first is the "diff-counter" class used to model the pulse 
counter A single slot, "current-count", which is initialized to zero, holds the cumulative 
sum of all delta theta orders and feedback. Its single method, "diff-signal", updates and 
returns the current count. 

count = count + order -feedbac (4.26) 

The "amplifier-clipper" class has three slots: "amplification-factor" (or gain), 
"max-value" and "min-value". The "amplify" method simply multiplies the argument by 
the gain and returns the product, clipped of course, to the maximum or minimum value if 
the product exceeds those prescribed limits. 

The "driver- class", a sub-class of amplifier-clipper, adds three gain slots for 
independent amplification of the three inputs before summing them in the final 
amplifier-clipper stage. The current feedback is not internal but relies on a saved slot 
value in the motor-class. 
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The "shaft" class is used as a superclass for the motor class, as sloth for reduction 
gear input and output shafts, and for the actual joint shaft. Slots angular-position (theta), 
angular-velocity (omega), inertia, coulomb-friction-constant (Fc or constant loss 
component), viscous-ffiction-constant (Fv or velocity dependent loss component), and 
time-stamp are defined. Methods are defined to provide capability to set theta and omega 
to some position and speed, reset theta and omega to zero, and couple (transfer) theta and 
omega to another shaft. 

The "motor" class inherits from the shaft class and defines additional slots: 
torque-constant (A' ( ), back-emf-constant ( K b ), armature-resistance ( RJ . armature-current 
(i a ), and brush drop parameters ( max-brush-drop and half-brush-drop-source -value). 
Methods "developed-torque" and "omega-dot" are direct implementations of Equations 
4.7 and 4.14, respectively. Brush drop was approximated by using an exponential form 
that approaches max-brush-drop as the source voltage increases [McPherson. 1 98 1 J. 

I'm = Vbdxms (1-0.5 Td/lUair-valae) (4.27) 

Method "run-motor" gets the elapsed time (dt), calculates omega-dot , then updates the 
motor state using Euler integrations. 

The "reduction-gear" class has slots "gear-ratio" (n), "in-shaft" and "out-shaft", 
the last two being instances of class shaft. Methods are provided to multiply or divide an 
argument by n (n squared if the argument is inertia). The method "rg-connect" is provided 
to replace the shaft couple method for internal coupling and reduces the coupled theta and 
omega by the gear ratio. 
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2. Joint Class 



The "joint" class actually represents a joint-actuator. It includes the entire 
system: motor controller, servo-motor (prime-mover), and reduction-gear. An additional 
shaft slot, "load-shaft", is actually only included to allow a convenient method to store the 
previous theta value in order to determine the feedback delta theta. The output shaft of 
the reduction gear actually holds the remaining load shaft slot values. The motor could 
likewise have been the reduction gear input shaft; however, they were kept separate for 
clarity. 

Methods are provided to sum system parameters external to the motor (i.e. 
load- inertia, ...) so they may be passed into calls to run- motor. "Feedback" returns the 
difference between the output shaft of the reduction gear and the load shaft. 
"Increment-joint" calls "run-motor" then couples all the shafts, being careful to save the 
old reduction gear output shaft position first for use in next call to feedback. 
"Step-input-to-joint" provides the facility to send delta theta orders to the pulse counter, 
and "reset-joint" reinitializes the system. 

Loading "joint.instance.cl" creates the instance of a joint used for model testing. 
The functions provided allow various orders to be sent to the joint and then displays 
system response to those orders. "Move-joint-mult" orders multiple repetitions of the 
same "delta-theta" order, each order being initiated upon completion of the previous 
order. "Move-joint-list" is similar in execution, but takes a list (sequence) of delta-theta's 
rather than repeating the same one. "Run-joint" orders a continuous sequence of small 
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delta-thetas, determined by argument "speed" and system elapsed time, required to achieve 
the ordered RPM speed. The remaining functions are intended to be internal calls. They 
are "clear-and-reset" (called by each of the previous three for initialization), 
"move-joint-list-2" (recursive call for move-joint-list), and "move-joint" (called by 
move-joint- mu It and move-joint-list-2). "Move-joint" is the workhorse and makes 
repeated calls to method "increment-joint" until the ordered delta-theta has been achieved 
(the pulse counter's current count approaches zero). In contrast, "run-joint" makes 
repeated calls to "increment-joint" until the ordered speed is achieved. 

3. Additional Supporting Code 

Loading "Window.instance.cl" creates a display window with a gradicule. A call 
to "display-state" reads the appropriate state slots and draws the set of data points for the 
current time. Additional methods are provided for internal calls and for reinitialization 
(i.e. clearing or resetting the window). The end result is a display of system state vs. time 
for program output. Finally, the file "joint-loader" is provided as a convenience, and 
loading it ensures loading of the source code files in the correct sequence (dependencies 
are observed). After loading the source code, it then makes calls that test run various 
features of the system. 

E. SUMMARY 

This chapter provides a review of the basic mathematics required to model 
servomotors and reduction gears and then describes the simulation of an Aquarobot joint 
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actuator. The model is simplified in that exception handling control signals are ignored 
and the pulse counter input is desired delta-theta for the joint shaft, but functionally, it is 
equivalent. In the next chapter, the Aquarobot model is presented; however, joint 
actuators are not yet included but instead are functionally represented by springs and 
dampers. 
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V. SPRING AND DAMPER MODEL 



\. INTRODL'CTION 

A modified dynamic simulation model for Aquarobot is developed in this chapter It 
has springs and dampers in place of servo motors to prov ide joint torques, flie springs, 
and dampers are intended to eventually be replaced by a joint actuator model such as that 
presented in the previous chapter Hie model can be tested by giving the Aquarobot an 
initial position and orientation and then allowing it to drop, observing the response as the 
feet contact the ground and the legs provide support. 

Since the purpose of the model is to provide an approximate dynamic simulation, 
capable of running in real time, sev eral simplifications have been made: 

( 1 ) the legs are considered to be massless; 

(2) the ground-foot friction is infinite (no slippage); 

(3) the center of mass is assumed to be at the geometric 
center of the inboard joints of the legs; and 

(4) body inertia is that of a solid homogeneous cylinder. 

In addition, joint stops (physical limits) in the kinematic model are disabled and collisions, 
other than feet contacting the ground, have been ignored. 
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B. INVERSE KINEMATICS 



The Aquarobot leg kinematic model allows determination of foot position, given the 
joint angles. The inverse kinematic problem, on the other hand, is to determine the joint 
angles, given the foot position. Using the appropriate coordinate systems simplifies 
inverse kinematics. Figure 5.1 illustrates the coordinate system used to calculate joint 1's 
angle, theta 1: the joint is the origin, the Z-axis is down (parallel to body Z-a.xis) and the 
X-axis is radially outward from the body. Theta 1 is measured as a right handed Z-axis 
rotation using the X-axis as a zero reference. 




Figure 5. 1 

Top View of Joint 1 Coordinate System Used to Calculate Theta 1. 

Given the foot position in this coordinate system, theta 1 is easily calculated: 

0! = arctan(^) . (5.1) 
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r 



joint3 




Side View of Coordinate System Used to Calculate Theta2 and IhetaS. 



Figure 5.2 illustrates the coordinate system used for calculating theta2 and thetaS Joint2 
serves as the origin, the X-axis is defined as the direction directly away from joint 1. and 
the Z-axis is again down. Using this coordinate system reduces the problem to two 
dimensions as the y-component of the foot position is now zero. Figure 5.3 and Equation 
5.2 illustrate the Icnv of cosines [Oakley, 1971] which is used to determine theta2 and 
theta3. 

ci~ — b‘ + c~ — 2bc cos (5.2) 




Figure 5.3 

Applying Law of Cosines 
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Solving Equation 5.2 for angle A: 

, T b- 1 

A = arccos L-^^J- 

Angles B and C are determined in the same manner: 



(5.3) 



B = arccos 



~ cr. -H~-b 2 1 

. ~c r 



(5.4) 

C - arccos (5.5) 

Referring back to Figure 5.3, sides a and b correspond directly to the lengths of link2 and 
Iink3. respectively, and side c may be calculated using Pythagorean's theorem: 



y ^ foot 4" - foot ■ (5.6) 

Angle D may be determined by several trigonomic functions; arcsin is used here: 

D = arcsin [- 7 ^] . (5.7) 

Theta2 and theta3 are measured using the sign convention shown. Note that theta3 is the 
negative of C's compliment: 

9 2 = B-D, (5.8) 

and 

0 3 = C-Ji. (5.9) 

Combining Equations, (5.4, 5.7 and 5 . 8 ) and (5.5 and 5.9), and substituting L2 and L3 for 
a and b results in 



02 = arccos 



03 - arccos 



~ arcsin [^], and 

~ i3+l\-c 2 I 



(5.10) 

(5.11) 
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C. JACOBIAN MATRIX 



The Jacobian Matrix, ,/ ( q ) or simply ./, of vector r with respect to vector ij is 
defined: 

= (5. 12) 

and is used to express the relation between an end effector velocity and the joint v elocities 
of a manipulator (Yoshikavva. 1990], In the case of Aquarobot. the foot velocitv with 
respect to leg joint v elocities is giv en by: 



(5.13) 



By rearranging Equation 5. 13. the inverse Jacobian and the foot velocity may be used to 
determine the joint velocities: 
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- foot 



(5.14) 



The Jacobian will also be used in the next section to determine ground reaction forces. 

The derivation of the Jacobian matrix for an Aquarobot leg is straightforward. The 
foot position of an Aquarobot leg is kinematically described as a function of the joint 
angles in [Schue, 1993]: 

.V = £ 0 cos9o +Licos9oi +Z,2cos9oicos9: +L3Cos9mcos9: , (5. 15) 

y = Losin9o + Tisin9 0 i +I:sin9oicos9: +Z.jsin9oicos9:3, (5. 16) 

r = — L; sin9 2 — L isin 9;3 , (5.17) 
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where L { is link, length, theta, is joint, angle, theta,, is the sum of theta, and theta,, and link,, 
represents the constant pseudo-link from center of body to joint, and has a joint angle 
equal to the "leg attachment angle". Differentiating Equations 5.15 through 5.17 gives: 

x = -L\ (sin0oi 0i ) -L:(sin0otcos02 0i +cos0oisin0; 0 : ) 

-L-? (sin 0oi cos 0;3 0 t + cos0oi sin02i(02 +0.i )) - (5.18) 

_v = —L i (cos 0oi 0i j - Z,2(cos0oicos02 0i +sin0oisin02 02 ) 

— L 3 (cos 0oi cos 0 23 9i + sin 0oi sin ©23(0 2 +03 )) , (5.19) 

■ = -L 2 (cos 02 02 ) — Li (cos 023 (o 2 +03 )) • (5.20) 



Regrouping and translating Equations 5.18 through 5.20 into the form of Equation 5.13, 
the Jacobian Matrix is given by 



J= 



~{L 1 + Z,2COS02 + Z,3COS0 23)sm0oi -(Z,2Sin02 - Z,3Sin023)COS0oi -l3COS0oiSin023 
(L i +^ 2 cos 02 Z. 3 cos 0 23 )cos 0 o i -(Z, 2 Sin 02 - L 3 sin 0 23 )sin 0oi -Z,3sin0oisin023 
0 — L 2 COS 02 — Z/ 3 COS 023 —Z, 3 COS 023 



(5.21) 



D. FORCES ON AQUAROBOT 

Assuming homogeneous cylindrical distribution of body mass and massless legs 
reduces the complexity of the forces and torques on Aquarobot. As Figure 5.4 illustrates, 

the resulting summations of these forces and torques may be expressed 

6 — ► 

/ Aquarobo, = "'S + ( 522 ) 
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and 




( 5 . 23 ) 



where f t is ground reaction force, and r Ug is the moment arm. from the body’s center of 
mass to the foot, on which that force is exerted. 



Given an initial known state (position, orientation and velocity), Aquarobot's motion may 
be completely described by application of Newton’s second law. The kinematic solution 
for r, is already available [Davidson. 1993], so all that remains is determination of 'f ltg . 

The torques at the joints in a manipulator are related to the force exerted by the end 
effector by the transpose of the Jacobian [Yoshikawa, 1990]: 




foot in contact 



Figure 5.4 

Forces on Aquarobot 




= [J) T f- 



(5.24) 
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where /is the force the foot exerts on the ground, equal magnitude but opposite direction 
of ground reaction force. To avoid confusion, / will thus refer to the ground reaction 
force, the force the ground exerts on the foot, as it is the force in which we are interested; 
therefore Equation 5.24 must have a sign change: 

(5.25) 

Solving Equation 5.25 for / yields: 

~f=~[J T \ X T. (5.26) 



E. SPRING AND DAMPER JOINT TORQUES 



In the spring and damper model, joint torque is the sum of spring restoring torque and 
damping torque: 

=(-*,(e-eo)) + (-*rfe), (5.27) 



t joint 



* damp 



where £ and k d are spring and damping constants, and thetaO is the ordered position. For 
the remainder of this chapter, thetaO will be taken as the rest position or zero torque 
reference. For convenience, the symbols on the right side of Equation 5.27 will be used to 
refer to their vector forms and will represent all three joints in a leg. Replacing the left 
side with T, and assuming the same spring and damping constants for each joint, 



T=-k s 



e, 

e 2 

03 



e>, 

02 „ 



03 



■k d 



01 

02 
03 



(5.28) 
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The joint angles are determined using the inverse kinematic method described above 
and are then available for the Jacobian matrix. For any foot in contact with the ground, its 
v elocity, relative to the body, is simply the negative of the sum of the body's translational 
v elocity and the cross product of the body's rotational v elocity with the foot's position 
vector. Combining Equations 5. 14 and 5.27. and substituting for foot velocity gives us: 

T k t (0 Go) + k ( j J [t boii\ + ^ ^ foot } ]. (5.29) 

which is now substituted into Equation 5.26 to give us the ground reaction force: 

— > ' -I 

f — J [A' y (0 ~ 0o) — A body ^ Pfoot)\ * (5.J>0) 

F. LISP PROTOTYPE 

The kinematic model for the Aquarobot simulation was borrowed from [Davidson. 
1993] with the only code modification being the conversion to Modified 
Danevit-Hartenberg (MDH) coordinate systems to match the C++ version. The complete 
source code listing with loading and operating instructions may be found in Appendix C. 
The additions required for dynamic operation of the model are best presented by the 
following walk-through of a dynamic update. Figure 5.5 provides a flowchart for 
reference. 

A dynamic update of Aquarobot is achieved by calling method "update-aquarobot". 
The body's acceleration, velocity and position are all updated by calls to previously defined 
methods in the rigid-body class. After these are completed, and the body is repositioned, 
the legs, forces and torques are then updated by calling "update- forces-and-torques" 
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Start 






Figure 5.5 

Flowchart for Dynamic Update of Aquarobot. 



Gravity is handled separately in the "update-acceleration" method; therefore, only the 
forces and torques due to foot contact with the ground need be considered. 
Update-forces-and-torques resets the body’s forces-and-torques vector, a rigid-body slot, 
to zeros, then calls "add-leg-forces-and-torques" for each leg to generate an updated 
cumulative value. 
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Add-leg-forces-and-torques tests for "foot-contact", a Boolean tvpe slot, or 
"new-contact", a function that sets foot-contact to true and the foot's world z coordinate 
to zero (ground level) if ground penetration is detected (z coordinate greater than zero). 
If there is no foot contact, nothing happens: joint angles remain set to their default v alues 
and the body's cumulative forces-and-torques value is left unchanged. If however, there is 
contact, the inverse kinematics routine is called, the ground reaction force is calculated 
using Equation 5.30, and the joint angles are updated. Before updating the cumulative 
forces-and-torques, loss of contact must be detected. This is done by testing the world z 
component of the calculated ground reaction force in a call to "still-in-contact" If the 
force is such that it is pulling the robot down rather than supporting it. then foot-contact is 
set to false, the joint angles are reset to their default values, and again, no contribution to 
forces-and-torques. If the foot is determined to be still-in-contact, world z component of 
the force less than zero (pushing up), then method "add-forces-and-torques-to-body" is 
called which adds / and r x f to the cumulative value of forces-and-torques as in 
Equations 5.22 and 5.23. After cycling through all the legs, Aquarobot is completely 
updated and ready for another cycle. 

This dynamic update cycle actually uses the (/ +I) rt velocity for the i lh update. While 
the i lh velocity might just as easily have been used, it is neither better nor worse. Better is 
actually the average of the two. 
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G. C++ PROTOTYPE 



The 0+ version of the dynamic Aquarobot model is algorithmically identical to the 
LISP version. A complete source code listing with operating instructions may be found in 
Appendix D. As discussed in Chapter III. use of IRIS Performer structures required some 
modifications. For example, six-vectors were divided into pairs of three-vectors. The 
dynamic model is otherwise identical. A feature added to this version is the ability to pass 
spring and damper constants, drop height, and dynamic update time increment into main 
as command line arguments. After handling these optional arguments, main initializes 
Performer, instantiates and initializes dynamic and graphic Aquarobot objects, then cycles 
in an update-render loop. 

The "graphic Aquarobot" object was not required in the LISP version as the "dynamic 
Aquarobot" slot values were directly accessed to render a stick figure. This version, 
however, draws thousands of filled polygons each cycle to render a single frame. IRIS 
Performer was used, as previously stated, primarily for its high performance in this task. 
Hie graphic Aquarobot is a hierarchical database containing the information required to 
draw Aquarobot. After each dynamic update, the body’s position and orientation and the 
leg's joint angles are passed into "Dynamic Coordinate Systems" in the database prior to 
calling the draw routine. 
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H. SUMMARY 



This chapter develops a simplified dynamic simulation model for Aquarobot using 
springs and dampers to provide the joint torques. Inverse kinematics. Jacobian matrices 
and their utility, and forces acting on Aquarobot are discussed The model was 
implemented and tested using both LISP and 0+. In the next chapter, simulation results 
of the spring and damper model, and also of the joint actuator model from Chapter IV. are 
presented. 
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VI. SIMULATION RESULTS 



A. INTRODUCTION 

This chapter presents the simulation results. The joint actuator simulation model was 
tested to verify the model and to experimentally determine suitable amplifier gains. As 
stated in the previous chapter, the spring and damper Aquarobot model was tested by 
dropping the robot from a low height and observing is dynamic behavior. 

B. JOINT ACTUATOR SIMULATION 

The motor class is designed such that constructor arguments are parameters listed on 
the motor specification sheet. At the time of the simulation, specification sheets for the 
motors used in Aquarobot were not available; therefore, another model was used. The 
model was tested by applying rated voltage to the motor and observing its acceleration 
and attained RPM. In Figure 6. 1, both motor and output shaft, after 200: 1 reduction gear, 
speeds and positions are shown. Motor scales are on the left, while output shaft scales are 
on the right. Qualitatively, the motor model is well behaved, and quantitatively, it closely 
matches the no-load speed parameter listed in the specifications. 
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Figure 6. 1 

No Load Joint Actuator Response With +75VDC Applied to the Motor 



To obtain fast joint actuator response to input orders, it is desirable to set the error 
signal, D/A converter output, gain to a relatively high value. A gain of 150 resulted in the 
optimum response. Values from 100 to 200 gave satisfactory results while higher values 
progressively reduced the effectiveness of the degenerative feedback. Figures 6 2 through 
6.5 show joint actuator responses to the following sequence of shaft positioning orders, in 
revolutions, with various driver gain values: (+1/4, +1/2, -1, -1/2, +1, +3). Figure 6.2 
illustrates the response with an error signal gain of 150 and with velocity and current 
feedback signals disabled. 
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Figure 6.2 

Joint Actuator Response with Error Signal Gain of 150. 
Velocity and Current Feedback Disabled. 



The optimum velocity feedback signal gain is a compromise between response time 
and stability. Too high a value results in slower response, while lower values allow 
increased overshoot. Values from three to seven were satisfactory. A gain of five proved 
optimum when combined with current feedback. Figure 6.3 displays the results of using 
various gain values for velocity feedback with current feedback still disabled. 
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Figure 6.3a 

Joint Actuator Response with Velocity Feedback Gain of 3 
and Error Signal Gain of 150. Current Feedback Disabled. 
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Figure 6.3b 

Joint Actuator Response with Velocity Feedback Gain of 5 
and Error Signal Gain of 150. Current Feedback Disabled. 
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Figure 6.3c 

Joint Actuator Response with Velocity Feedback Gain of 7 
and Error Signal Gain of 150. Current Feedback Disabled. 



joint actuator simulation 




Figure 6.3d 

Joint Actuator Response with Velocity Feedback Gain of 20 
and Error Signal Gain of 150. Current Feedback Disabled. 
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The current feedback gain value turned out to be the most sensitive A value of 0 3 
produced negligible change, while 0.7 resulted in some oscillation This regenerative 
feedback, intended to reduce response time, also aided in reducing the overshoot As the 
error signal approached zero with speed on, back EMF caused a decelerating current 
which was aided by the regenerative current feedback Figure 6 4 displays the results of 
using various gain values for current feedback with velocity feedback again disabled In 
addition, in the presence of the degenerative velocity feedback, the error signal was 
overcome earlier by decelerating signals, bringing the current feedback contribution in 
even sooner. Figure 6.5 illustrates results with both velocity and current feedbacks in 
effect. The response of the joint actuator in this simulation highlights the effectiveness of 
the design: high error signal gain with velocity and current compensating feedback 
networks. 

While these tests results are not for the specific motors used in Aquarobot, they 
provide a good set of gain parameters to use as a starting point in the experimental 
determination of the gains for those motors. 
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Figure 6.4a 

Joint Actuator Response with Current Feedback Gain of 0.3 
and Error Signal Gain of 150. Velocity Feedback Disabled. 
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Figure 6.4b 

Joint Actuator Response with Current Feedback Gain of 0.5 
and Error Signal Gain of 150. Velocity Feedback Disabled. 
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Figure 6.4c 

Joint Actuator Response with Current Feedback Gain of 1.0 
and Error Signal Gain of 150. Velocity Feedback Disabled 
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Figure 6.5a 

Joint Actuator Response with Velocity Feedback Gain of 3, 
Error Signal Gain of 150, and Current Feedback Gain of 0.5. 
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Figure 6.5b 

Joint Actuator Response with Velocity Feedback Gain of 5, 
Error Signal Gain of 150, and Current Feedback Gain of 0.5. 
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Figure 6.5c 

Joint Actuator Response with Velocity Feedback Gain of 7, 
Error Signal Gain of 150, and Current Feedback Gain of 0.5. 
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Figure 6.5d 

Joint Actuator Response with Velocity Feedback Gain of 10, 
Error Signal Gain of 150, and Current Feedback Gain of 0.5. 
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C. AQUAROBOT SPRING AND DAMPER SIMULATION 

LISP and C++ versions of the spring and damper Aquarobot model were tested by 
using a "droptest" in which the model is dropped from low height. It may be tilted, but 
not so much that it does not land on its feet. The LISP version served as the prototype, 
and after successful testing, the model was translated into C++. 

The LISP simulation ran uncompiled on a Sun Sparc- 10 at six to eight frames per 
minute and was too slow for comprehension of motion detail. It did, however, allow 
experimental determination of spring and damper constants sufficient to support the model 
when dropped. To increase the simulation speed, two dynamic update cycles were run 
between each display, and the source code was then compiled. After compilation, the 
simulation ran at near 30 frames per minute, with 60 dynamic updates of 50ms each, to 
achieve a simulation with approximately a 10:1 time dilation. This simulation was fast 
enough for an observer to assimilate the dynamic behavior of the model which was 
qualitatively satisfactory and considered successful. Some additional fine tuning of 
experimentally determined parameters was done prior to translation to C++. 

After translation to C++/Performer, the model was again tested, and a real-time 
simulation was achieved on a four processor IRIS 440/RE workstation. Only three 
processors were actually utilized, one assigned to each of the following tasks: application, 
database pre-draw cull, and database rendering traversal. The application processor 
utilization was approximately fifteen percent which indicates that the increased complexity 
of adding the joint actuators will not present any difficulty. The cull processor utilization 
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was approximately twenty percent while that of the rendering processor veried from fifty 
to seventy percent. (Note: the source code was not compiled and/or linked with 
optimizations on ) Typical images obtained can be found in [Goetz, 1994] 

Running in the three processor configuration described above, Performer 
synchronized the framerate to the fixed 50ms dynamic updates by limiting the framerate to 
20Hz. On a single processor IRIS R-4000, where the application, database cull, and 
database rendering were forced to run sequentially, the highest framerate achieved was 
10Hz. This resulted in a 2: 1 time dilation (slow down) simulation. 

D. SUMMARY 

Testing of both simulation models was considered successful. While it is unfortunate 
that there was insufficient time to incorporate the joint actuator model into the Aquarobot 
model, replacing the springs and dampers, the topic was given some time and effort. This 
next step is among the topics addressed in the final chapter: Summary and Conclusions 
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VII. SUMMARY AND CONCLUSIONS 



A. UTILITY OF LISP FOR EXPERIMENTAL PROGRAMMING 

The primary benefit of using LISP as a prototyping language in this thesis was the 
immediate testing capability it provided. No test routines were required. Each function 
was easily tested by direct calls as it was developed. While compilation capability allowed 
a faster simulation, repeated compilations were not required as the routines could called 
from the interpreter's command line. Finally, nesting functions allowed larger and larger 
integrated blocks of source code to be tested. 

One of the benefits of using LISP during prototyping was realized when an apparent 
limit cycle appeared if Aquarobot was tilted when dropped. The problem was actually a 
lack of rotational damping due to the absence of the omega cross r term in the foot 
velocity calculation. Without it, damping ceased shortly after landing because of "zero 
translational foot velocity." While the author took a considerable period of time to find 
the cause of the problem, with a simple modification to the LISP source code, the 
correction was quickly and easily verified. 

B. INCORPORATING THE JOINT ACTUATOR MODEL 

The next step required for the Aquarobot dynamic simulation model is to replace the 
springs and dampers with joint actuators. This was initially assumed to be a simpler task 
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than it turned out to be. The difficulty arises from a conflict over control of joint state 
variables. In the spring and damper model, the joints only supply state dependent torques 
which are then used to dynamically update the robot's body. After the body is updated, 
the new states of the joints depend only on the new body position and not on the joint 
torques In the joint actuator model, the joint state depends directly upon the motor 
torque. Two possible solutions are proposed to eliminate the conflict. 

One possible solution is to extend the massless leg simplification to the motor and 
gear-train, making them inertialess. While this seems easiest and will probably have as 
little impact as the massless legs, the overall effect may be greater than anticipated due to 
the large reduction ratios involved. Recall that motor inertia reflected outside the 
reduction gear is multiplied by the square of the reduction ratio. A C++ version of the 
joint actuator model is included in Appendix B This model is a variation of the original 
LISP version and provides state dependent torques as output rather than the state itself It 
could be used to implement inertialess joint actuators, and the changes required in the 
Aquarobot model would be minor. 

Another possible solution is to use the concept of "added mass," an apparent increase 
in mass (affecting acceleration) due to the internal inertia of the drive motors. Assuming a 
unit acceleration for one of the body's six degrees of freedom, it is possible to calculate the 
resulting joint accelerations. If a joint acceleration is known, inertial torque in the joint 
can be calculated. The net torque for the joint then is armature torque (determined using 
motor applied voltage and speed) minus the inertial torque. Doing this for all six degrees 
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of freedom gives a matrix of added mass which can be inverted to get acceleration for any 
given vector of joint motor applied voltages. An "equilibrium torque," torque vector 
which results in a zero joint acceleration vector, is also needed and must be calculated 
[Koozekanani, 1983], 

C. SUMMARY 

IRIS Performer has proven its utility as a graphics rendering tool in a real time 
simulation. It also provided easy synchronization for fixed duration integration intervals. 

A real-time dynamic simulation of Aquarobot was accomplished and is eventually 
expected to provide a valuable tool for Aquarobot control software developers. While an 
integrated model, with the joint actuators in place, is not yet completed, we are a step 
closer, and the task has certainly proven to be feasible. Once the joint actuators are 
installed into the Aquarobot model, simple walking simulations, on smooth, flat terrain 
may be achieved. Concurrent work on collision detection for uneven terrain will also 
further improve the simulation model when incorporated [Goetz, 1994], 

Further work could improve on the Performer Aquarobot rendering database to 
decrease the rendering time. This is the area where there is the most room for 
improvement in cycle time. Finally, other than using the "faster" Performer routines, no 
attempt has been made to optimize the source code which was written with ease of 
modification in mind. Utilization of compile and link optimizations, and eventually some 
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source code tuning, may also contribute toward achievement of a real-time Aquarobot 
simulation on a single processor 
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APPENDIX A 



LOADING AND OPERATING INSTRUCTIONS 



To run demo, start LISP Interpreter and call: 
(load "joint-loader”) 



SOURCE CODE FILES 
; M joint-Ioader M 



; load files 

(load "math. routines. cl”) 

(load "time. routines. cl") 

(load "diff-counter.class.cl") 

(load "amplifier-clipper.class.cl") 
(load "shaft. class. cl") 

(load "motor. class.cl") 

(load "reduction-gear.class.d") 
(load "joint. class. cl") 

(load "joint. instance.cl") 

(load "window. instance. cl") 

; execute tests 
(move-joint-mult -.25 4) 
(move-joint-mult .25 4) 
(move-joint-mult .05 4) 
(move-joint-list '(.25 .5 -1 -.5 1 3)) 
(run-joint 15) 
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; M diff-counter\class.cr’ 



(defclass diff-counter () 

((current-count 

:accessor current-count 
initform 0 
type float ))) 

(dcfmcthod diff-signal ( (dc dilT-counter) plus-input minus-input) 
(setf (current-count dc) 

(^ (current-count dc) plus-input (- minus-input)))) 



; ,, amplifier-cIipper.class.cl M 



(defclass amplifier-clipper () 

((amplification-factor 

mitarg amplification-factor 
accessor amplification-factor 
:initform 1 
type float) 

(max-value 
nnitarg : max-value 
:accessor max-value 
:initform 1 
:type float) 

(min-value 
:initarg : min-value 
:accessor min-value 
initform -I 
:t\pe float))) 

(defmethod amplify ((amp amplifier-clipper) input-value) 
(max (min (* (amplification-factor amp) input-value) 
(max-value amp)) 

(min-value amp))) 
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(defclass motor-driver (amplifier-clipper) 

((displacement-gain 

mitarg :displacement-gain 
accessor displacement-gain 
initform 1 
type float) 

(velocity-fb-gain 
mitarg : velocity-fb-gain 
; accessor velocity-fb-gain 
initform 0 
type float) 

(current-fb-gain 
mitarg :current-fb-gain 
accessor current-fb-gain 
initform 0 
type float))) 

(defmethod drive((driver motor-driver) displacement-input 
velocity-input 
current-input) 

(amplify dnver (+ (* displacement-input (displacement-gain driver)) 
(* (- velocity-input) (velocity-fb-gain dnver)) 

(* current-input (current-fb-gain driver))))) 
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; "shaft. class. cl” 



(defclass shaft () 

((angular-position , radians 

accessor theta 
mitform 0 
type float) 

(angular-velocit) , rad/sec 

accessor omega 
mitform 0 
t\pe float) 

(inertia , Kg-(meters-square) 

mitarg :I 
accessor 1 
uniform 0 
:type float) 

(coulomb-fnction-constant ; Newton-meters (Fc >= 0) 

:initarg Fc 
accessor Fc 
mitform 0 
type float) 

(viscous-friction-constant Newton-rneters/(rad/sec) (F\ >= 0) 
mitarg :Fv 
accessor Fv 
mitform 0 
type float) 

(time-stamp ; seconds 

accessor time-stamp 
mitform (system-time)))) 

(defmethod set-shaft ((s shaft) theta omega) 

(setf (omega s) omega) 

(setf (theta s) theta)) 

(defmethod reset-shaft ((s shaft)) 

(set-shaft s 0 0) 

(setf (time-stamp s) (system-time))) 

(defmethod connect ((source shaft) (load shaft)) 

(setf (time-stamp load) (time-stamp source)) 

(set-shaft load (theta source) (omega source))) 
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; "motor.class.d M 



(defclass motor (shaft) 

((torque-constant ; Newton-meters/ampere 
:imtarg ;Kt 
:accessor Kt 
:t\pe float) 

(back-emf-constant , Volts/(rad/sec) 

: initarg Kb 
accessor Kb 
:t>pe float) 

(armature-resistance ; ohms (must be > 0) 
initarg R 
: accessor R 
initform 1 
t\pe float) 

(max-brush-drop ; volts 
initarg : max-brush-drop 
accessor max-brush-drop 
initform 2.0 
:t\pe float) 

(half-brush-drop-source-value ; volts 
initarg :half-BD-value 
accessor half-BD-value 
initform 3.0 
type float) 

(armature-current ; amperes (saved for feedback purposes) 

: accessor armature -current 
:initform 0 
type float))) 

(defmethod applied-voltage ((m motor) source-voltage) 

(if (zerop source-voltage) 0 
(* source-voltage 

M 

(* (/ (max -brush-drop m) (abs source-voltage)) 

(-1 

(exp (♦ (log 0.5) 

(/ (abs source-voltage) 

(half-BD-value m)))))))))) 

(defmethod developed-torque ((m motor) source-voltage) 

(setf (armature-current m) 

(/ (- (applied-voltage m source-voltage) (* (Kb m) (omega m))) (R m))) 

(* (Kt m) (armature-current m))) 
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(defmethod omega-dot ((m motor) source-voltage load-inertia load-torque 
load-coulomb-friction-constant 
load-viscous-friction-constant) 

(let* ((torque (^ (developed-torque m source-voltage) load-torque)) 
(Fc-total (+ (Fc m) load-coulomb-friction-constant)) 

(fnction-loss 
(if (zerop (omega m)) 

(if (zerop torque) 

0 

(if (> Fc-total (abs torque)) 
torque 

(* Fc-total (sgn torque)))) 

(+ (* ( + (Fv m) load-viscous-friction-constant) (omega m)) 

(* Fc-total (sgn (omega m))))))) 

(/ (- torque friction-loss) (+ (1 m) load-inertia)))) 

(defmethod run-motor ((m motor) source-voltage load-inertia load-torque 
load-coulomb-fnction-constant 
load-viscous-friction-constant) 

(let ((dt (delta-time (time-stamp m))) 

(omega-dot (omega-dot m source-voltage load-inertia load-torque 
load-coulomb-friction-constant 
load-viscous-friction-constant))) 

(setf (theta m) (+ (theta m) (* (omega m) dt))) 

(setf (omega m) (+ (omega m) (* omega-dot dt))) 

(setf (time-stamp m) (+ (time-stamp m) dt)))) 
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; "reduction-gear.class.cl" 



(defclass reduction-gear () 

((gear-ratio 
amtarg :gear-ratio 
accessor gear- ratio 
initforni 1 
:t\pe float) 

(in-shaft 
mitarg in-shaft 
accessor in-shaft 
initform (make- instance 'shaft)) 

(out-shaft 
initarg :out-shaft 
:accessor out-shaft 
initform (make-instance ’shaft)))) 

(defmethod rg-reduce ((rg reduction-gear) value) 

(/ value (gear-ratio rg))) 

(defmethod rg-reflect ((rg reduction-gear) value) 

(* value (gear-ratio rg))) 

(defmethod rg- inertia-forward ((rg reduction-gear) inertia-value) 

(* inertia-value (sqr (gear-ratio rg)))) 

(defmethod rg-inertia-backward ((rg reduction-gear) inertia-value) 
(/ inertia-value (sqr (gear-ratio rg)))) 

(defmethod rg-connect ((rg reduction-gear)) 

(set-shaft (out-shaft rg) 

(rg-reduce rg (theta (in-shaft rg))) 

(rg-reduce rg (omega (in-shaft rg))))) 
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; ’’joint. class. cl” 



(defclass joint () 

((pulse-counter 
anitarg pc 
accessor pc 

initform (make-instance ’diff-counter)) 

(driver 

initarg driver 
accessor driver 

initform (make-instance 'motor-driver)) 

(prime-mover 
initarg prime-mover 
accessor prime-mover 
initform (make-instance ’motor)) 

(red-gear 
initarg red-gear 
accessor red-gear 

initform (make-instance ’reduction-gear)) 

(load-shaft 
: initarg : load-shaft 
accessor load-shaft 
initform (make-instance ’shaft)))) 

(defmethod motor-load-inertia ((j joint)) 

(+ (I (in-shaft (red-gear j))) 

(rg-inertia-back\vard (red-gear j) (+ (I (out-shaft (red-gear j))) 
(I (load-shaft j)))))) 

(defmethod motor-load-coulomb-friction-constant ((j joint)) 

(+ (Fc (in-shaft (red-gear j))) 

(rg-reduce (red-gear j) (+ (Fc (out-shaft (red-gear j))) 

(Fc (load-shaft j)))))) 

(defmethod motor-load-viscous-friction-constant ((j joint)) 

(+ (Fv (in-shaft (red-gear j))) 

(rg-reduce (red-gear j) (+ (Fv (out-shaft (red-gear j))) 

(Fv (load-shaft j)))))) 

(defmethod feedback ((j joint)) 

(- (theta (out-shaft (red-gear j))) (theta (load-shaft j)))) 
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(defmethod increment-joint ((j joint) order) 

(run-motor (prime-mover j) 

(drive (driver j) (diff-signal (pc j) order (feedback j)) 

(* .003 (RAD/SECtoRPM (omega (prime-mover j)))) 
(armature-current (prime-mover j))) 
(motor-load-inertia j) 

0 ; load not producing any torque 
(motor-load-coulomb-friction-constant j) 
(motor-load-viscous-fnction-constant j)) 

(connect (out-shaft (red-gear j)) (load-shaft j)) 

(connect (prime-mover j) (in-shaft (red-gear j))) 

(rg-connect (red-gear j))) 

(defmethod step-input-to-joint ((j joint) step) 

(diff-signal (pc j) (REVtoRAD step) 0)) 

(defmethod reset-joint ((j joint)) 

(setf (current-count (pc j)) 0) 

(reset-shaft (load-shaft j)) 

(reset-shaft (out-shaft (red-gear j))) 

(reset-shaft (in-shaft (red-gear j))) 

(reset-shaft (prime-mover j))) 
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; "joint. instance.cl" 



(self joint 1 (make-instance joint 

;driver (make-instance 'motor-driver 
:amplification-factor 1 
: max-value 75 ; volts 

nun-value -75 ; volts 

displacement-gain 150 
velocity-lb-gain 5 

:current-fb-gain .5 ) 

prime-mover (make-instance 'motor 



1 


.0005 , Kg-m*m 


Fc 


0075 ; N-m 


F\ 


.00004 ; N-m/(rad/sec) 


:Kt 


005 , N-m/ampere 


Kb 


255 ; Volts/( rad/sec) 


:R 


1 ) ; ohms 


(make-instance 'reduction-gear 



gear-ratio 200) 

load-shaft (make-instance 'shaft 
:I 5 ; Kg-m*m 

Fc 1 ; N-m 

:Fv .02 ))) ; N-m/(rad/sec) 

(defiin move-joint (delta-theta) 

(setf (time-stamp (prime-mover joint 1)) (system-time)) 

;input delta-theta a little at a time 
(if (< delta-theta 0) 

;negative delta-theta (use -0.015 steps) 

(do* ((input-index 0 (+ input-index 0.015))) 

((< (+ delta-theta input-index) 0.015) 

(step-input-to-joint joint 1 (+ delta-theta input-index))) 
(step-input-to-joint joint 1 -0.015) 

(increment-joint joint 1 0) 

(display-state *display* joint 1 (time-stamp (prime-mover joint 1)))) 
ipositive delta-theta (use 0.015 steps) 

(do* ((input-index 0 (+ input-index 0.015))) 

((< (- delta-theta input-index) 0.015) 

(step-input-to-joint joint 1 (- delta-theta input-index))) 
(step-input-to-joint joint 1 0.0 1 5) 

(increment-joint jointl 0) 

(display-state *display * joint 1 (time-stamp (prime-mover jointl))))) 
, delta-theta entry into PC is complete 
;cycle until ordered position is reached 
(do* ((indexl)) 

((and (< (abs (current-count (pc jointl))) 0.05) 

(< (abs (omega (prime-mover jointl))) 10)) (pprint ’stop)) 
(increment-joint jointl 0) 

(display-state *display* jointl (time-stamp (pnme-mover jointl))))) 
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(defun move-joint-mult (delta-theta mult) 

(clear-and-reset) 

(dotimes (i mult) (move-joint delta-theta))) 

(defun run-joint (speed) 

(clear-and-reset) 

(do* ((dtime (delta-time (time-stamp (prime-mover joint 1))) 

(delta-time (time-stamp (pnme-mover joint 1))))) 

((< (abs (- (RPMtoRAD/SEC speed) (omega (load-shaft joint 1)))) 

0 1) (ppnnt 'stop)) 

(increment-joint joint 1 (* dtime (RPMtoRAD/SEC speed))) 
(display-state *display* jointl (time-stamp (prime-mover joint 1 ))))) 

(defun move-joint-list (delta-theta-list) 

(clear-and-reset) 

(if (equalp ml delta-theta-list) nil 
(move-joint-list-2 delta-theta-list))) 

(defun move-joint-list-2 (delta-theta-list) 

(move -joint (car delta-theta-list)) 

(if (equalp ml (cdr delta-theta-list)) nil 
(move-joint-list-2 (cdr delta-theta-list)))) 

(defun clear-and-reset () 

(update-minutes *display* 0) 

(reset-system-time) 

(reset-joint jointl)) 



; "window.instance.d" 



; dimensions for x-y coord system (window size auto adjusts) 
(setf *x-ongin* 50) 

(setf *x-length* 500) 

(setf *x-tics* 6) 

(setf *y-origin* 50) 

(setf *y-length* 340) 

(setf *y-tics* 8) 

(setf * max-speed* 4000) ; (max rpm's of motor scale) 

(setf *max-revs* 1000) ; (max rev's of motor scale) 

(require :xcw) 

(use-package :cw) 

(cw:initjalize-common-windows) 
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(defmethod draw-grid ((window window -stream)) 

(draw-line-xy window *x-origin* .top border 

(+ *y-origm* *y-length*) 

(^ *x-origin* *x-Icngth*) 

(-*- *y-origin* *y-length*)) 

(draw-line-xy window *x-origin* .middle x axis 

(^ *y-origin* (/ *y -length* 2)) 

(- *\-origm* *x-length*) 

{+ *\ -origin* (/ *\ -length* 2))) 

(draw-line-xy window *x-origin* .bottom border 

* \ -origin* 

(+ *x-ongin* *x-length*) 

*\ -origin*) 

(draw-line-xy window *x-ongin* deft border 

*\ -origin* 

*x-origin* 

(+• *y-origin* *y-length*)) 

(draw-line-xy window (+ yx -origin* *x-length*) right border 
*y-origm* 

(+ *x-origin* *x-lenglh*) 

(+ *y-origin* *y-length* )) 

(dotimes (i *x-tics*) , mark x axis 

(draw-line-xy window (+ *x-origin* 

(/(* i *x-length*) *x-tics*)) 

*y-origin* 



(+ *x-ongin* 

(/ (* i *x-!ength*) *x-tics*)) 

(+ *y-origin* *y-length*) 
dashing '( 1 3))) 

(dotimes (i *\ -tics*) . mark y axis 

(draw-line-xy window *x-ongin* 

(f *y-ongin* 

(/ (* i *y-length*) *y-tics*)) 

(+ *x-ongin* *x-length*) 

(+ *y-origin* 

(/ (* l *y-length*) *y-tics*)) 
dashing ’(1 3))) 

(label-graph window)) 
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(defmethod label-graph ((w window-stream)) 

, theta labels 

(setf (window-stream-y-position w) (+ *v-ongin* *y-length* -4)) 

(setf (window-stream-x-position w) (+ *x-origin* -35)) 

(setf (window-stream-foreground-color w) red) 

(format w "-s" *max-revs*) 

(setf (window-stream-x-position w) (+ *x-origin* *x-length* 5)) 

(setf (window -stream-foreground-color vv) blue) 

(format w "+1”) 

(setf (vvindovv-stream-y-position vv) (+ *y-ongin* (* *y-length* 0.75) -3)) 
(setf (window-stream-x-position w) (+ *x-origin* -40)) 

(setf (window -stream-foreground-color \v) black) 

(format w "Revs”) 

(setf (window-stream-x-position w) (+ *.\-ongin* *x-length* 5)) 

(format w "Revs”) 

(setf (window-stream-y-position w) (+ *y-origin* (* *y-length* 0.5) 3)) 
(setf (window-stream-x-position vv) (+ *x-ongin* -43)) 

(setf (window -stream-foreground-color w) red) 

(format vv "~s" (- *max-revs*)) 

(setf (window-stream-x-position w) (+ *x-ongin* *x-length* 5)) 

(setf (window-stream-foreground-color w) blue) 

(format w ”-l”) 

, omega labels 

(setf (windovv-stream-y-position w) (+ -origin* (* *y-length* 0.5) -10)) 
(setf (window-stream-x-position w) ( 4 - *x-ongin* -35)) 

(setf (window-stream-foreground-color w) red) 

(format w "~s” * max-speed*) 

(setf (window-stream-x-position w) (+ *x-origin* *x-length* 10)) 

(setf (window -stream-foreground-color w) blue) 

(format w "-s" (/ *max-speed* 100)) 

(setf (window-stream-v-position w) (+ *y-origin* (* *y-length* 0.25) -3)) 
(setf (window-stream-x-position vv) (+ *x-origin* -36)) 

(setf (window-stream-foreground-color w) black) 

(format w "RPM”) 

(setf (window-stream-x-position w) (+ *x-origin* *x-length* 5)) 

(format w "RPM”) 

(setf (window-stream-y-position w) (+ *y-origin* 3)) 

(setf (window-stream-x-position w) (+ *x-origin* -43)) 

(setf (window-stream-foreground-color w) red) 

(format w "~s" (- *max-speed*)) 

(setf (window-stream-x-position w) (+ *x-origin* *x-length* 5)) 

(setf (window-stream-foreground-color w) blue) 

(format w "~s" (- (/ *max-speed* 100))) 

; time labels 

(setf (window-stream-foreground-color w) black) 

(setf (window-stream-y-position w) (+ *y-origin* -15)) 

(setf (window-stream-x-position w) (+ *x-origin* -3)) 

(format w "0") 

(setf (window-stream-x-position w) (+ *x-origin* (/ *x-length* 6) -7)) 
(format vv "10”) 

(setf (window-stream-x-position w) (+ *x-origin* (/ *x-length* 3) -7)) 



72 



(format w "20") 

(self (window-slream-x-position w) (-*- *x-origin* (/ *\-lengih* 2) -7)) 
(format w "30”) 

(self (window -stream-x-position w) (+ *x-origin* (* *x-length* (/ 2 3)) -7)) 
(format w ”40”) 

(self (window-stream-x-position w) (+ *x-origin* (* *x-length* (/ 5 6)) -7)) 
(format w "50") 

(self (window -stream-x-position \v) (+ *x-origin* *x-length* -7)) 

(format w "60”) 

(self (window -stream-y -position w ) ( + *y-origin* -30)) 

(self (window -stream-x-position \v) (+ *x-origm* -35)) 

(self (window -stream-foreground-color w) red) 

(format w "motor") 

(setf (window-stream-x-position w) (+ *x-origin* *x-length*)) 

(self (window-stream-foreground-color w) blue) 

(format w "joint") 

(setf (window -stream-foreground-color \v) black) 

(seif (window-stream-x-position w) (+ *x-ongin* (* *x-length* 5) -13)) 
(format vv "time")) 

(dcfmethod draw-motor-position ((window window-stream) revolutions) 
(draw-point-xy window 
*x-time-value* 

(+ *y-origm* (* 0 75 *y-length*) 

(*0.25 *y-lenglh* 

(cond ((zerop revolutions) 0) 

((< revolutions 0) 

(/ (- (mod revolutions * max-revs*) 

* max-revs*) 

* max-revs*)) 

(t (/ (mod revolutions *max-revs*) 

* max-revs*))))) 

color red)) 

(defmethod draw -load-position ((window window-stream) revolutions) 
(draw-point-xy window 
*x-time-value* 

(+ *y-origin* (* 0.75 *y-length*) 

(* 0.25 *y-length* 

(cond ((zerop revolutions) 0) 

((< revolutions 0) 

(-(mod revolutions 1.0) 1.0)) 

(t (mod revolutions 1.0))))) 
color blue)) 
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(defmethod draw-motor-speed ((window window-stream) speed) 
(dravv-point-xv window 
*x-time-value* 

(+ *y-ongin* (* 0.25 *v-length*) 

(* 0.25 *y-length* (/ speed * max-speed*))) 

:color red)) 

(defmethod draw -load-speed ((window w indow -stream) speed) 
(draw-point-xy window 
*x-time-value* 

(+ *v-origin* (* 0.25 *y-length*) 

(* 0.25 *y-length* 100 (/ speed * max-speed*))) 
color blue)) 

(defmethod update-minutes ((window' w indow -stream) minutes) 

(clear window) 

(draw-grid window) 

(setf *minutes* minutes)) 

(defmethod set-x-coord ((window window-stream) seconds) 

(if (> (truncate (/ seconds 60)) *minutes*) 

(update-minutes window (truncate (/ seconds 60)))) 

(setf *x-time-value* (round (+ *x-ongin* 

(* (/ (mod seconds 60) 60) 

*x-length*))))) 

(defmethod display-state ((window window-stream) (j joint) current-time) 
(set-x-coord window current-time) 

(draw-motor-speed window (RAD/SECtoRPM (omega (prime-mover j)))) 
(draw -load-speed window (RAD/SECtoRPM (omega (load-shaft j)))) 
(draw-motor-position window (RADtoREV (theta (prime-mover j)))) 
(dravv-load-position window (RADtoREV (theta (load-shaft j))))) 

(setf *display* 

(make-window-stream 
deft 1 
:bottom 1 

: width (+ *x-length* (* 2 *x-origin*)) 

:height (+ *y-length* (* 2 *y-origin*)) 

:background-color white 
: foreground-color black 
: inner-region-left 
: inner-region-bottom 
: inner-region- width 
: inner-region-height 
:title "joint actuator simulation" 

:activate-p t)) 

(setf *minutes* 0) 

(draw-grid *display*) 
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; "time.routines.cr 



(defun reset-svstem-time () 

(self *RetTime* (get-intemal-real-time))) 

(defun system-time () 

(/ (- (get-internal-real-time) *RefTime*) 1000.0)) 

(defun delta-time (time) 

(- (system-time) time)) 

(reset-sy stem-time) 



; "math. routines. cl" 



(defun sqr (x) (* x x)) 

(defun sgn (x) 

(if (< x 0) -1 l)) 

(defun RPMtoRAD/SEC (rpm) 

(* rpm (/ pi 30))) ; * 2pi/60 

(defun RAD/SECtoRPM (rad/sec) 
(* rad/sec (/ 30 pi))) 

(defun REVtoRAD (rev) 

(* 2 pi rev)) 

(defun RADtoREV (rad) 

(/ rad (* 2 pi))) 
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APPENDIX B 



SOURCE CODE FILES (untested) 



//file "motor.h" 



#ifndef MOTORH 

^define _MOTOR_H 

#include <math.h> 
#include <stdio h> 



class motor { // inertia-less motor class 
private: 

float Fc; // Coulomb friction constant 
float Fv; // Viscous friction constant 

float Kt; // Torque constant 

float Kb; // Back EMF constant 

float Ra; // Armature resistance 

float BDm; // Rated brush brop value 
float BDc; // Ln(l/2)/BDh 

// BDh = Voltage applied such that brush drop = 1/2 BDm 
// subtracts brush drop from source voltage 
float AppliedVoltage(float); 



public: 

float la; // Armature current (available for current feedback) 
motor() { } 

// called after constructor for initialization 
void init_motor(float TorqueConstant, // N*m/ Ampere 

float BackEMFConstant, // Volts/RPM 

float NoLoadCurrent, // Amperes 

float NoLoadSpeed = 1000.0, // RPM 
float StartingCurrent = 0.0, // Amperes 
float ArmatureResistance = 1.0, // Ohms 

float RatedBrushDrop = 2.0, // Volts 

float HalfBrushDrop = 3.0);// Volts 
float DevelopedTorque(float, float); 

}; 
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// provide initialization for specific motor types in Aquarobot 
void makcRA20(motor& in); 
void makeRH25(motor& m), 

#endif 
/* EOF */ 



// file "motor.c" 



^include "motor. h" 

void motor imt_motor( float TorqueConstant. 
float BackEMFConstant, 
float NoLoadCurrent, 
float NoLoadSpeed, 
float StartingCurrent, 
float ArmatureResistance, 
float RatedBrushDrop, 
float HalfBrushDrop) 

{ 

la = 0.0; 

Fc = TorqueConstant * StartingCurrent; 

Fv = (TorqueConstant * NoLoadCurrent - Fc) / NoLoadSpeed, 

Kt = TorqueConstant; 

Kb = BackEMFConstant; 

Ra = ArmatureResistance; 

BDm = RatedBrushDrop; 
if (BDm < 0.0) { 

printf("error: rated bnish drop must be >= 0 Volts.. An"); 
printf("default brush drop value (0 Volts) used...\n"); 

BDm = 0.0; 

BDc = 1.0; 

} 

else if (BDm = 0.0) { 

BDc = 1.0; 

} 

else if (HalfBrushDrop < BDm/2) { 
printf("error: half brush drop must be >= 1/2 rated brush drop...\n"), 
printf("defaulted to 1/2 RatedBrushDrop... \n M ); 

BDc = log(0.5) ♦ 2.0 / BDm; 

} 

else { 

BDc = log(0.5) / HalfBrushDrop; 

} 

} 
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float motor: :DevelopedTorque(float SourceVoltage, float omega) 

{ 

float Torque; 
float FnctionLoss; 

// determine armature current and save 

la = (AppliedVoltage(Source Voltage) - (Kb * omega)) / Ra; 

// determine motor torque 
Torque = Kt * la; 

// calculate loss 

// loss opposes omega (viscous and coulomb components) 
if (omega > 0.0) FrictionLoss = omega * Fv + Fc; 
else if (omega < 0.0) FrictionLoss = omega * Fv - Fc; 

// if (omega == 0) : loss opposes Torque (no viscous component) 
else if (Torque > Fc ) FrictionLoss = Fc; 
else if (Torque < -Fc) FrictionLoss = - Fc; 

// if ((omega == 0) && (|Torque| < Fc)) : Torque insufficient to overcome Fc 
else FrictionLoss = MotorTorque; 

return (Torque - FnctionLoss); 

} 



78 



/* Private Function */ 



float motor :AppliedVoltage( float Vs) 

{ 

//return Vs(l - (BDm/|Vs|)*(l - e\p(ln( 1/2)* Vs|/BDh))) 

if (Vs == 0.0) { 
return 0 0, 

/ 

else if (Vs < 0.0) { // negative Vs 
return (Vs + (BDm * { 1 - e.\p(-BDc*Vs)))); 

x 

else { // positive Vs 

return (Vs - (BDin * ( 1 - e\p( BDc* Vs)))). 



* specific Aquarobot motor type initializations */ 

// Aquarobot motor parameters for imt_motor 

// read from spec sheet prov ided, includes harmonic gear 

#define RA20_PARAMETERS 32.0, 3.4, 0.78, 25.0, 0.32, 3.2, 2.0, 3.0 

^define RH25_PARA METERS 33.0, 3.5, 0 89, 25.0, 0.48, I 1, 2.0, 3.0 

void makeRA20(motor& m) 

{ 

m . i nit_motor( RA20_P ARAMETERS ) ; 

} 



void makeRH25(motor& m) 

{ 

m. imt_motor(RH25_PARAJVlETERS); 

} 



/* EOF */ 
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//file "amplifierclipper.h" 



#ifndef AMP_CLIP_H 

#define _AMP_CLIP_H_ 

class amplifier_clipper { 

pnvate: 
float gain; 
float max_value; 
float minvalue; 

public: 

amplifler_clipper() { } 

// call after constructor for initialization 

void init_amplifier_clipper(float g, float max_v, float min_v) 

float amplify(float input_value); 

}; 

class aqua driver : private amplifier_clipper { 
private: 

float theta_gain; 
float omega_gain; 
float current gain; 

public: 

aqua_driver() { } 

// call after constructor for initialization 
imt_aqua_driver(float displacement_gain, 
float velocity_gain, 
float current_feedback_gain); 
float dnve(float theta error, float omega, float current); 

}; 



// provide initialization for specific joint on aqualeg 
void makeJldriver(aqua_dnver& d); 
void makeJ2driver(aqua_driver& d); 
void makeJ3driver(aqua_driver& d); 

#endif 
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// file "amplifier clipper.c” 



^include "amplifierjrlipper.h" 
void 

init_amplifier_clipper( float g, float niax v, float nun_v) 

{ 

gain = g; 
max_value = ma\\, 
min value = min v; 



float 

ampliiier_clipper :amplify( float input_value) 

{ 

float output_value = gain * input_value; 

if (nun vaiue > output_value) 
return min_value; 
elsif (maxvalue < output_value) 
return max_value; 
else 

return output_value; 

} 

aqua_driver::init_aqua_dnver(float displacement_gain, 
float velocity _gain, 
float current_feedback_gain) 

{ 

// set final gain to 1 and clip at +/- 75 VDC 
init_amplifier_clipper(1.0, 75.0, -75.0); 

thetagain = displacement_gain; 
omegagain = velocitygain; 
current_gain = currentfeedbackgain; 

} 

float 

aqua_driver::dnve(float theta_error, float omega, float current) 

{ 

return (amplify( theta_gain * theta_error 
- omega_gain * omega 
+ current_gain * current)); 
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// driver specs (theta, omega, current) 

^define SHOULDER_DRIVER_GAINS 150.0. 5 0. 0 5 
#define KNEE 1DR1 VER GAINS 150.0, 5.0. 0.5 
#defme KNEE2_DRIVER_GAINS 150 0. 5.0. 0.5 

void makeJldriver(aqua_dnver& d) 

/ 

X 

d mit_aqua_dnver( SHOULDER DR1 VER_GAINS); 

} 



void makeJ2dnver(aqua_dnver& d); 

/ 

\ 

d imt_aqua_dnver(KNEEl_DRIVER_GAINS); 

} 

void makeJ3driver(aqua_driver& d), 

{ 

d.init_aqua_driver(KNEE2_DRIVER_GAINS); 

} 



// file ”joint_actuator.h" 

#ifndef JA_H 

#define JA_H 

#include <> 

^include MM 

class aquajointactuator { 
protected: 

float orderedtheta; 

amplifier_clipper da_converter; 
amplifier_clipper fV_converter; 
aqua_dnver d; 
motor m; 

public: 

aqua Joint_actuator() { } 

void reset(float theta); 

void input_order(float delta_theta); 

float torque(float currenttheta, float currentomega); 

}; 



class shoulder_actuator : public aquaJoint_actuator { 
public: 

shoulder_actuator(float theta); 

}; 
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class kneel_actuator public aquajointjictuator { 
public: 

kneel_actuator(float theta); 

float torque(float current theta. float current jmiega); 

}; 



class knee2_actuator public aqua joint_actuator { 
public: 

knee2_actuator(float theta); 

float torque( float current jheta, float current junega); 



#endif 



// file "jointactuator.c" 



^include "joint jictuator h" 

// aquajoint_actuator (parent class) functions 
void 

aquajoint actuator reset( float theta); 

{ 

ordered_theta = theta; 
m.Ia = 0.0; 

} 

void 

aquajoint_actuator::mput_order(float delta theta); 

{ 

ordered_theta += delta_theta; 

} 

float 

aquaJoint_actuator::torque(float currentjheta, float current_omega), 

{ 

float sourcej'oltage; 
source joltage = d->drive 

(da_converter->amplify(current Jheta - orderedjheta), 

fv_converter->amplif\(current_omega), 

m.Ia); 

return (m->developcdjorque(source joltage)); 
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// derrived class specs 



// output 10 volts for 6144 pulse count 
^define DA_CONVERTER_RATIO 10.0/6144.0 

// 100 pulses drive the motor 1 revolution 
^define PULSES_PER_REV 100.0 

// output 3 volts per 1000 RPMs 

#define FV_C0NVERTER_RAT10 3.0/1000.0 

// harmonic gear only for shoulder 
^define SHOULDERREDUCTION 161.0 

// harmonic and bevel gears for knees 
#define KNEE1_REDUCTI0N 160.0*3.0 
#define KNEE2_REDUCTION 160.0*2.0 

// demved class functions 

shoulder_actuator::shoulder_actuator(float theta) 

{ 

ordered_theta = theta; 
da_converter->init_amplifier_clipper 

(SHOULDER_REDUCTION * PULSES_PER_REV * DAJ30NVERTERRATI0, 10.0, -10.0); 
fv_converter->init_amplifier_clipper 
(SHOULDER_REDUCTION * FV_C0NVERTER_RAT10, 10.0, -10.0); 
makeJldriver(&d); 
makeRA20(&m); 

} 

knee 1 actuator : :knee 1 _actuator( float theta) 

{ 

orderedjheta = theta, 
da_converter->init_amplifier_clipper 

(KNEE 1REDUCTION * PULSES_PER_REV * DA_CONVERTER_RATIO, 10.0, -10.0); 
fV_converter->init_amplifier_clipper 
(KNEE l_REDUCTION * FV_CONVERTER_RATIO, 10.0, -10.0); 
makeJ2driver(&d); 
makeRH25(&m); 

} 

float 

kneel_actuator::torque(float current_theta, float current_omega) 

{ 

return (3.0 * aquaJoint_actuator::torque(current_theta, current_o mega/3.0)); 

} 
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knee2 jictuator knee2_actuator( float theta) 

r 

i 

orderedjheta = theta, 
da_converter->inu_amplifierclipper 

(KNEE2_REDUCTION * PULSES_PER_REV * DA_CON\ r ERTER_RATIO. 10.0. -10 0). 
fV_converter->initjiirtplifier_clipper 
(KN r EE2_REDUCTI0N * FV_CONVERTER_RATIO. 10 0, -10 0). 
makeJ3driver(&d); 
makeRH25(&m); 



float 

knee2_actuator :torque( float current jheta, float current omega) 

i 

\ 

return (2.0 * aquajoint_actuator::torque(curreni_theta. current_omega/2 0)). 
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APPENDIX C 



LOADING AND OPERATING INSTRUCTIONS 



To run demo, start LISP Interpreter and call: 

(load "aqua-loader") 

This file loads the source code in the correct sequence and makes calls to run the demo. 
Additional runs may be observed by calling: 

(drop-aqua) 



SOURCE CODE FILES 



; "aqua-loader" 

; LOADER FOR AQUA-ROBOT 

, define loader/compiler functions 

(load " load-files. cl") 

, aqua-robot loader/compiler functions 

, (load-aqua) 

(load-compiled-aqua) 

; (compile-and-load-aqua) 

(defun drop-aqua () 

(restart-aqua) 

(do 0 (nil) 

(dotimes (i ♦loops*) (update-aquarobot aqua-1)) 
(new-picture))) 

(aqua-picture) 

(drop-aqua) 
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; ’’load-files. cl M 



(defun load-aqua () 

, general purpose files 
(load "niisc.cn 
(load "vector.cl") 

(load "matrix cl") 

(load "kinematics. cl") 

(load "rigid-body cl") 

(load "strobe-camera cl") 

(load "link. cl") 

. aqua-robot specific files 
(load "aqua-data. cl") 

(load "aqua-link. cl") 

(load "aqua cl") 

(load "aqua-leg. cl") 

(load "aqua-inv-kinematics cl") 

(load "aqua -Jacobian. cl") 

(load "aqua-update-forces-and-torques.cl")) 

(defun load-compiled-aqua () 

(load "misc.fasl") 

(load "vector. fasl") 

(load "matrix. fasl") 

(load "kinematics. fasl") 

(load "rigid-body. fasl") 

(load "strobe-camera fasl") 

(load "link. fasl") 

(load "aqua-data. fasl") 

(load "aqua-link. fasl") 

(load "aqua.fasl") 

(load "aqua-leg. fasl") 

(load "aqua-inv-kinematics.fasl") 

(load "aqua-jacobian.fasl") 

(load "aqua -update-forces-and-torques. fasl")) 
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(defun compile-and-load-aqua () 

; (compile-file "misc.d") 

(load "misc.fasr) 

; (compile-file "vector.d") 

(load "vector.fasl") 

; (compile-file '‘matrix. cl") 

(load "matrix. fasl") 

(compile-file "kinematics. cl") 

(load "kinematics. fasl") 

(compile-file "rigid-body. cl") 

(load "rigid-body fasl") 

(compile-file "strobe-camera. cl") 

(load "strobe-camera. fasl") 

(compile-file "link. cl") 

(load "link. fasl") 

(compile-file "aqua-data. cl") 

(load "aqua-data. fasl") 

(compile-file "aqua-link. cl") 

(load "aqua-link. fasl") 

(compile-file "aqua. cl") 

(load "aqua.fasl") 

(compile-file "aqua-leg.cl") 

(load "aqua-leg. fasl") 

(compile-file "aqua-inv-kinematics.d") 

(load "aqua-inv-kinematics.fasr') 

(compile-file "aqua-jacobian.cl") 

(load "aqua-jacobian.fasl") 

(compile-file "aqua-update-forces-and-torques.cl") 
(load "aqua-update-forces-and-torques.fasl")) 



88 



; "misc.cl M 



(dcfun atan2 (dx dy) 

(cond ((zerop dx) (cond ((zerop dy) 0.0) 

((< dy 0) (-(*0.5 pi))) 

(t (*0.5 pi)))) 

((< dx 0) (cond ((< d\ 0) (- (atan (/ dy dx)) pi)) 
(t (+ (atan (/ dy dx)) pi)))) 

(t (atan (/ d> dx))))) 

returns angle in degrees 

(defun atan2d (dx dy) (rad-to-deg (atan2 dx d> ))) 
(defun sqr (x) (* x x)) 

(defconstant rad-to-deg-multiplier (7 180 pi)) 

(defun rad-to-deg (rad) (* rad rad-to-deg-multiplier)) 

(defconstant deg-to-rad-multiplier (/ pi 180)) 

(defun deg-to-rad (deg) (* deg deg-to-rad-multiplier)) 

, Returns first n elements of list. 

(defun near (n list) 

(cond ((zerop n) nil) 

(t (cons (car list) (near (1- n) (edr list)))))) 
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; "vector.cl" 



; A vector is a list of numerical atoms. 

(defun vector-add (vector-1 vector-2) (mapcar '+ vector- 1 vector-2)) 

(defun vector-subtract (vector-1 vector-2) (mapcar vector-1 vector-2)) 

(defun scalar-multiply (scalar vector) 

(cond ((null vector) nil) 

(t (cons ( * scalar (car v ector)) 

(scalar-multiply scalar (cdr vector)))))) 

(defun dot-product (x y) 

(apply ’+ (mapcar '* x y))) ;A matrix is a list of row vectors. 

(defun cross-product (x y) ;x and y are 3D vectors. 

(list (- (* (cadr x) (caddr y)) (* (caddr x) (cadr y))) 

(- (* (caddr x) (car y)) (* (car x) (caddr y))) 

(- (* (car x) (cadr y)) (* (cadr x) (car y))))) 

(defun vector-length (vector) (sqrt (dot-product vector vector))) 

(defun distance-betvveen (x y) ;points x and y represented by vectors, 
(vector-length (vector-sub tract x y))) 

; returns a vector (O*(one-position -1)1 O*(length-one-position)) 
(defun umt-vector (one-position length) 

(do ((n length (1- n)) 

(vector nil (cons (cond ((= one-position n) 1) (t 0)) vector))) 

((zerop n) vector))) 

(defun append 1 (L) (append L '(!))) 



90 



; "matrix. cl" 



; requires VECTOR CL 
; requires MISC.CL "near” 

, A matrix is a list of row vectors. 

(defun transpose (A) 

(cond ((null (edr A)) (mapear 'list (car A))) 

(t (mapear 'cons (car A) (transpose (edr A)))))) 

(defun post-multiplv (M x) ;M is a square matrix, \ is a conformable vector 
(cond ((null (edr M)) (list (dot-product (car M) x))) 

(t (cons (dot-product (car M) x) (post-multiplv (edr M) \))))) 

(defun pre-multiply (vector matrix) 

(post-multiplv (transpose matrix) vector)) 

(defun matrix-multiply (A B) ,A and B are conformable matrices. 

(cond ((null (edr A)) (list (pre-multiply (car A) B))) 

(t (cons (pre-multiply (car A) B) (matrix-multiply (edr A) B))))) 

(defun chain-multiply (L) ;L is a list of names of conformable matrices 
(cond ((null (eddr L)) (matrix-multiply (eval (car L)) (eval (cadr L)))) 

(t (matrix-multiply (eval (car L)) (chain-multiply (edr L)))))) 

(defun cycle-left (matrix) (mapear ’rovv-cycle-left matrix)) 

(defun rovv-cy cle-left (row) (append (edr row) (list (car row)))) 

(defun cycle-up (matrix) (append (edr matrix) (list (car matrix)))) 

(defun unit-matrix (size) 

(do ((row-number size (1- row-number)) 

(I nil (cons (unit-vector row -number size) I))) 

((zerop row-number) I))) 

(defun concat-matnx (A B) ;A and B are matrices with equal number of rows 
(cond ((null A) B) 

(t (cons (append (car A) (car B)) (concat-matnx (edr A) (edr B)))))) 

(defun augment (matrix) 

(concat-matnx matnx (unit-matrix (length matrix)))) 

(defun normalize-row (row) (scalar-multiply (/ 1.0 (car row)) row)) 
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(defun solve-first-column (matrix) , Reduces first column to ( 1 0 0). 

(do* ((LI matrix (cdr LI)) 

(L2 (normalize-row (car matrix))) 

(L3 (list L2) (cons (vector-add (car LI) 

(scalar-multiply (- (caar LI)) L2)) L3))) 

((null (cdr LI)) (reverse L3)))) 

(defun square-car (M) ;Retums square matrix extracted from front of matrix M 
(do ((m (length M)) 

(LI M (cdr LI)) 

(L2 nil (cons (near m (car LI)) L2))) 

((null LI) (reverse L2)))) 

..L is a list of lists This function finds list with 
largest car and moves it to head of list of lists. 

(defun max-car-first (L) 

(cond ((null (cdr L)) L) 

(t (if (> (abs (caar L)) (abs (caar (max-car-first (cdr L))))) L 
(append (max-car-first (cdr L)) (list (car L))))))) 

^Applies max-car-first to first n elements of list. 

(defun nmax -car-first (n list) 

(append (max -car-first (near n list)) (nthedr n list))) 

(defun matrix-inverse (M) 

(do ((Ml (max-car-first (augment M)) 

(cond ((null Ml) nil) 

(t (nmax -car-first n (cycle-left (cycle-up Ml)))))) 

(n (1- (length M)) (1- n))) 

((or (minusp n) (null Ml)) (cond ((null Ml) nil) (t (square-car Ml)))) 

(setq Ml (cond ((zerop (caar Ml)) nil) (t (solve-first-column Ml)))))) 
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; "kinematics. cl” 



requires MATRIX CL 

(defun dh-matrix (cosrotate sinrotate costwist sintwist length translate) 
(list (list cosrotate (- (* costwist sinrotate)) 

(* sintwist sinrotate) (* length cosrotate)) 

(list sinrotate (* costwist cosrotate) 

(- (* sintwist cosrotate)) (* length sinrotate)) 

(list 0. sintwist costwist translate) (list 0. 0. 0. I ))) 

(defun mdh-mntrix (cosrotate sinrotate 
costwist-i-1 sintwist-i-1 
length-i- 1 translate) 

(list (list cosrotate (- sinrotate) 0 length-i-1) 

(list (* sinrotate costwist-i-1) (* cosrotate costwist-i-1) 

(- sintwist-i-1) (- (* sintwist-i-1 translate))) 

(list (* sinrotate sintwist-i-1 ) (* cosrotate sintwist-i-1) 
costwist-i-1 (* costwist-i-l translate)) 

(list 0. 0. 0. 1.))) 

(defun homogeneous-transform (azimuth elevation roil x y z) 
(rotation-and-translation (sin azimuth) (cos azimuth) (sin elevation) 
(cos elevation) (sin roll) (cos roll) x y z)) 

(defun rotation-and-translation (spsi cpsi sth cth spin cplu x y z) 

(list (list (* cpsi cth) (- (* cpsi sth sphi) (* spsi cpln)) 

(+ (* cpsi sth ephi) (* spsi sphi)) x) 

(list (* spsi cth) (+ (* cpsi ephi) (* spsi sth sphi)) 

(- (* spsi sth ephi) (* cpsi sphi)) y) 

(list (- sth) (* cth sphi) (* cth ephi) z) 

(list 0 . 0 . 0 . 1.))) 

(defun inverse-H (H) 

(let* ((minus-P (list (- (fourth (first H))) 

(- (fourth (second H») 

(- (fourth (third H))))) 

(inverse-R (transpose (square-car (reverse (rest (reverse H)))))) 
(inverse-P (post-multiply inverse-R minus-P))) 

(append (concat-matnx inverse-R (transpose (list inverse-P))) 

(list (list 0 0 0 L))))) 
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; "rigid-body. cl" 



, requires KINEMATICS. CL 
(defclass rigid-body () 

((location ;The three-vector (x v z) in world coordinates, 

initarg location 
accessor location) 

(velocity ;The six-vector (u v w p q r) in body coordinates, 

initarg velocity 
accessor velocity) 

(acceleration The vector (u-dot v-dot w-dot p-dot q-dot r-dot). 
accessor acceleration) 

(forces-and-torques ;The vector (Fx Fy Fz L M N) in body coordinates 
:accessor forces-and-torques) 

(moments-of-inertia ;The vector (lx Iy Iz) in principal axis coordinates, 
initarg :moments-of-inertia 
accessor moments-of-inertia) 

(mass 

initarg :mass 
:accessor mass) 

(node-list ;List of vertices for wire frame model 

initarg inode-list 
:accessor node-list) 

(polygon-list ;Sets of above vertices defining polygons 
:imtarg :polygon-list ;Ex:'((l 2 3)(2 3 4 5)(4 5 6)) 
accessor polygon-list) 

(transformed-node-list 
:accessor transformed-node-list) 

(H-matrix 
:accessor H-matrix) 

(current-time 
:accessor current-time))) 

(defmethod move (fbody rigid-body) azimuth elevation roll x y z) 

(setf (H-matrix body) 

(homogeneous-transform azimuth elevation roll x y z)) 
(transform-node-list body) 

(update-position body)) 
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(defmethod move-incremental ((body rigid-bodv ) increment-list) 
(setf (H-matnx body) 

(matrix-multiplv (H-matrix body) (homogeneous-transform 
(first increment-list) 

(second increment-list) 

(third increment-list) 

(fourth increment-list) 

(fifth increment-list) 

(sixth increment-list)))) 
(transform-node-list body ) 

(update-position body)) 



(defmethod get-delta-t ((body rigid-body)) 

(let* ((new -time (gct-intcrnal-rcal-time)) 

(delta-t (/ (- new-time (current-time body)) 1000))) 

(setf (current-tune body) new-time) 
delta-t)) 

(defmethod start-timer ((body rigid- body)) 

(setf (current-time body) (get-intemal-real-time))) 

(defmethod update-ngid-bodv ((body rigid-body)) , Euler integration 
(let* ((delta-t (get-delta-t body))) 

(update-H-matnx body delta-t) 

(transform-node-list body) 

(update-position body) 

(update-velocity’ body delta-t) 

(update-acceleration body))) 

(defmethod update-acceleration ((body ngid-bodv)) 

(setf (acceleration body) ;(list u-dot v-dot w-dot p-dot q-dot r-dot) 
(multiple-value-bind ; Assumes pnncipal axis 

(Fx Fy Fz L M N uvwpqr lx Iy Iz) ; coordinates with origin at 
(values-list ;center of gravity of body. 

(append 

(forces-and-torques body) (velocity body) (moments-of-inertia body))) 
(list (+ (* v r) (* -1 w q) (/ Fx (mass body)) 

(* *gravity* (first (third (H-matnx body))))) 

(+ (* w p) (* -1 u r) (/ Fy (mass body)) 

(* * gravity* (second (third (H-matnx body))))) 

(+ (* u q) (* -1 v p) (/ Fz (mass body)) 

(* *gravity* (third (third (H-matnx body))))) 

(/ (+ (* (- Iy Iz) q r) L) lx) 

(/ (+ (* (- Iz lx) r p) M) Iy) 

(/ (+ (* (- lx Iy) p q) N) Iz))))) 
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(defmethod update-velocity ((bod\ rigid-body) delta-t) 

(setf (velocity body) 

(vector-add (velocity body) 

(scalar-multiplv delta-t (acceleration body))))) 

(defmethod update-H-matrix ((body rigid-body) delta-t) 

(setf (H-matrix body) 

(matnx-multiply 

(H-matnx body) (homogeneous-transform 
(* delta-t (sixth (velocity' body))) 

(* delta-t (fifth (velocity body))) 

(* delta-t (fourth (velocity body))) 

(* delta-t (first (velocity body))) 

(* delta-t (second (velocity body))) 

(* delta-t (third (velocity body))))))) 

(defmethod transform-node-list ((body rigid-body)) 

(setf (transformed-node-list body) 

(mapcar #'(lambda (node-location) 

(post-multiply (H-matrix body) node-location)) 

(node-list body)))) 

(defmethod update-position ((body rigid-body)) 

(setf (location body) (near 3 (first (transformed-node-list body))))) 

; (defconstant * gravity* 32.2185) 

(defmethod world-to-body ((body rigid-body) xyz-pos) 

(near 3 (post-multiply (inverse-H (H-matrix body)) 

(append xyz-pos '(!))))) 

(defmethod body-to-world ((body rigid-body) xyz-pos) 

(near 3 (post-multiply (H-matrix body) (append xyz-pos '(I))))) 
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; "strobe-camera. cl” 



requires RIGID-BODY CL 

(require :xc\v) 

(cw initialize-common-windows) 

(defclass strobe-camera (ngid-bod\) 

((focal-length 
accessor focal-length 
lmtform 6) 

(camera-window 
accessor camera-window 
mitform (cw make-w indow -stream borders 5 
left 500 
bottom 500 
width 300 
height 300 

title "strobe-camera-image" 

:activate-p t)) 

(H-matrix 

inuform (homogeneous-transform .3 -.3 0 -300 -90 -90)) 

(inverse-H- matrix 
accessor inverse-H-matnx 

initform (inverse-H (homogeneous-transform .3 -.3 0 -300 -90 -90))) 
(enlargement-factor 
accessor enlargement-factor 
;initform 30))) 

(defmethod erase-camera-window r ((camera strobe -camera)) 

(cwxlear (camera-window camera))) 

(defmethod move ((camera strobe-camera) azimuth elevation roll x y z) 

(setf (H-matrix camera) (homogeneous-transform azimuth elevation roll x y z)) 
(setf (inverse-H-matnx camera) (inverse-H (H-matnx camera)))) 

(defmethod take-picture ((camera strobe -camera) (body rigid-body)) 

(let ((camera-space-node-list (mapcar #'(lambda (node-location) 

(post-multiply (inverse-H-matrix camera) node-location)) 
(transformed-node-list body)))) 

(dolist (polygon (polygon-list body)) 

(clip-and-draw -polygon camera polygon camera-space-node-list)))) 
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(defmethod clip-and-draw-polygon 
((camera strobe-camera) polygon node-coord-list) 

(do* ((initial-point (nth (first polygon) node-coord-list)) 

(from-point initial-point to-point) 

(remaining-nodes (rest polygon) (rest remaining-nodes)) 

(to-point (nth (first remaining-nodes) node-coord-list) 

(if (not (null (first remaining-nodes))) 

(nth (first remaining-nodes) node -coord-list)))) 

((null to-point) 

(draw -clipped-projection camera from-point initial-point)) 
(draw-clipped-projection camera from-point to-point))) 

(defmethod draw -clipped-projection ((camera strobe-camera) from-point to-point) 
(cond ((and (<= (first from-point) (focal-length camera)) 

(<= (first to-point) (focal-length camera))) ml) 

((<= (first from-point) (focal-length camera)) 

(draw-line-in-camera-window' camera 
(perspective-transform camera (from-clip camera from-point to-point)) 
(perspective-transform camera to-point))) 

((<= (first to-point) (focal-length camera)) 

(draw -line-in-camera-window camera 
(perspective-transform camera from-point) 

(perspective-transform camera (to-clip camera from-point to-point)))) 

(t (draw-line-in-camera-window camera 
(perspective-transform camera from-point) 

(perspective-transform camera to-point))))) 

(defmethod from-clip ((camera strobe-camera) from-point to-point) 

(let ((scale-factor (/ (- (focal-length camera) (first from-point)) 

(- (first to-point) (first from-point))))) 

(list (+ (first from-point) 

(* scale-factor (- (first to-point) (first from-point)))) 

(+ (second from-point) 

(* scale-factor (- (second to-point) (second from-point)))) 

(+ (third from-point) 

(* scale-factor (- (third to-point) (third from-point)))) 1))) 

(defmethod to-clip ((camera strobe-camera) from-point to-point) 

(from-clip camera to-point from-point)) 

(defmethod draw-line-in-camera-window ((camera strobe-camera) start end) 
(cw:draw-line (camera-window camera) 

(cw: make-position :x (first start) :y (second start)) 

(cw: make-position :x (first end) :y (second end)) 
ibmsh-width 0)) 
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(defmethod perspective-transform ((camera strobe-camera) point-in-camcra-spacc) 
(let* ((enlargement-factor (enlargement-factor camera)) 

(focal-length (focal-length camera)) 

(x (first pomt-in-camcra-space)) \ axis is along optical axis 
(y (second point-in-camcra-spacc)) ,y is out right side of camera 
(z (third point-in-camera-space))) ,z is out bottom of camera 
(list (+ (round (* enlargement-factor (/ (* focal-length y) x))) 

150) .to right in camera window 

(+ 150 (round (* enlargement-factor (/ (* focal-length (- z)) \)) 

))))) .tip in camera window 
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"link.cl" 



; requires RIGED-BODY.CL 

(defclass link (rigid-body) 
((motion-limit-flag 
initform nil 

accessor motion-limit-flag) 
(twist-angle 
initarg twist-angle 
accessor twist-angle) 

(link-length 
mitarg : link-length 
accessor link-length) 

( inboard-joint-angle 
initarg inboard-joint-angle 
accessor inboard-joint-angle) 
(inboard-joint-displacement 
i nitarg : inboard-joint-displacement 
:accessor inboard-joint-displacement) 
(inboard-link 
initarg :inboard-link 
:accessor inboard-link) 

(A-matnx 
:accessor A-matrix) 

; added for mdh 
(twist-angle-i-1 
:initarg :twist-angle-i-l 
: accessor twist-angle-i-l) 
(link-length-i-1 
initarg : link-length-i-1 
:accessor link-length-i-1) 

(T-matrix 

:accessor T-matrix))) 

(defclass rotary-link (link) 
((min-joint-angle 
: initarg : min-joint-angle 
.accessor min-joint-angle) 
(max-joint-angle 
:initarg :max-joint-angle 
:accessor max-joint-angle))) 
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(defclass sliding-link (link) 
((min-joint-displaccment 
initarg :min-joint-displacemcnt 
accessor min-joint-displacement) 
(max -joint-displacement 
:initarg : max -joint-displacement 
accessor max-joint-displacement))) 
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; "aqua-data. cl" 



, load after MISC.CL 



(defconstant *dt* 0.01) ; delta-t each update, 
(defconstant *loops* 2) ; updates between draws. 

(defconstant linkOlength 37.5) 

(defconstant link 1 length 20.0) 

(defconstant link21ength 52.0) 

(defconstant link31ength 102.0) 

(defconstant flag-length 25.0) 

, leg attachment angles. 

(defconstant leg 1 -angle (deg-to-rad 0)) 
(defconstant leg2-angle (deg-to-rad 60)) 
(defconstant leg3-angle (deg-to-rad 120)) 
(defconstant leg4-angle (deg-to-rad 180)) 
(defconstant leg5-angle (deg-to-rad 240)) 
(defconstant leg6-angle (deg-to-rad 300)) 



initial position and orientation in world coordinates, 
(defconstant azimuth-init (deg-to-rad 0.0)) 
(defconstant elevation-init (deg-to-rad 0.0)) 
(defconstant roll-imt (deg-to-rad 9.0)) 
(defconstant x-init 0.0) 

(defconstant y-init 0.0) 

(defconstant z-init -135.0) 



; initial (default) joint angles. 

(defconstant joint 1-init (deg-to-rad 0.0)) 

(defconstant joint2-init (deg-to-rad 25.0)) 

(defconstant joi nt3 -init (deg-to-rad -115.0)) 

(defconstant default-angles (list joint 1-init joint2-init joint3 -init)) 

; joint spnng constants, (fill in : Kg-cm2/sec2 per radian) 

(defconstant jointl-K -2000000) ; 5000000 = Scott’s 500 Nm per radian, 
(defconstant joint2-K -2000000) 

(defconstant joint3-K -2000000) 

(defconstant spring-constants (list jointl-K joint2-K joint3-K)) 

Joint spnng damping constants, (fill in : Kg-cm2/sec2 per radian/sec) 
(defconstant joint 1-D -800000) ; 800000 = Scott’s 80 Nm-sec per radian/sec. 
(defconstant joint2-D -800000) 

(defconstant joint3-D -800000) 

(defconstant spring-damping-constants (list jointl-D joint2-D joint3-D)) 
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; joint limits. 

;(defconstant joint 1-imn-limit (deg-to-rad -60.0)) 

;(defconstant joint 1 -max-limit (deg-to-rad 60.0)) 

;(defconstant joint2-min-limit (deg-to-rad - 106.6)) 

;(defconstant joint2-max-limit (deg-to-rad 73.4)) 

;(defconstant joint3-inin-limit (deg-to-rad -156.4)) 

;(defconstant joint3-max-limit (deg-to-rad 23.6)) 

(defconstant joint 1-imn-limit -50.0) 

(defconstant joint 1 -max-limit 50.0) 

(defconstant joint2-min-limit -50.0) 

(defconstant joint2-max-limit 50.0) 

(defconstant joint3-min-limit -50.0) 

(defconstant joint3-max-linut 50 0) 

; mass in Kg. 

(defconstant aqua-bod\-mass 500.0) 

;(defconstant link 1 mass 0.0) 

; (defconstant link2mass 0.0) 

;(defconstant link3mass 0.0) 

; (lx Iy Iz)-Kg-cm. in pnncipal axis coordinates 
, assumes solid cylindrical body of constant densit\ 

(defconstant aqua-body-height 50.0) 

(defconstant aqua-body-radius 30.0) 

(defconstant aqua-body-Ix 

(+ (* (/ 1 4) aqua-body-mass (sqr aqua-body-radius)) 

(* (/I 12) aqua-body-mass (sqr aqua-body-height)))) 

(defconstant aqua-body-Iy aqua-bodv-lx) 

(defconstant aqua-body-Iz (* (/ 1 2) aqua-body-mass (sqr aqua-bod> -radius))) 
(defconstant aqua-body-inertia (list aqua-body-Ix aqua-body-Iy aqua-body-Iz)) 

; center of mass. 

(defconstant body-mass-center '(0 0 0)) 

; (defconstant link I mass-center (list (/ link 1 length 2) 0 0)) 

;(defconstant link2mass-center (list (/ link21ength 2) 0 ())) 

;(dcfconstant link3 mass-center (list (/ link31ength 2) 0 0)) 

(defconstant ^gravity* 980.0) ;cm/sec/sec. 
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; "aqua-link.d" 



; requires LINK. CL 
; requires AQUA-DATA CL 

(defclass linkO (rotary-link) 

((twist-angle anitform 0) 

(link-length imtform linkOlength) 

(inboard-joint-angle anitform 0) 
(inboard-joint-displacement :imtform 0) 
(min-joint-angle anitform (deg-to-rad -360)) 
(max-joint-angle anitform (deg-to-rad 360)) 

(node-list anitform (list (list 0 0 0 1) (list 0 0 0 1) 

(list linkOlength 0 0 1))) ; for mdh 

(list (- linkOlength) 0 0 1))) ; for dh 
(polygon-list anitform ’((1 2))))) 

(defclass link 1 (rotary-link) 

((twist -angle anitform (deg-to-rad -90)) 

(link-length anitform linkl length) 

(inboard-joint-angle anitform joint 1 -init) 
(inboard-joint-displacement anitform 0) 
(min-joint-angle anitform joint 1-min-limit) 
(max-joint-angle anitform joint 1 -max-limit) 

(node-list anitform (list (list 0 0 0 1) (list 0 0 0 1) 

(list linkllength 0 0 1))) ; for mdh 

(list (- linkllength) 0 0 1))) ; for dh 
(polygon-list anitform ’((1 2))))) 

(defclass link2 (rotary-link) 

((twist-angle anitform 0) 

(link-length anitform link21ength) 

(inboard-joint-angle anitform joint2-init) 
(inboard-joint-displacement anitform 0) 
(min-joint-angle anitform joint2-min-limit) 

(max -joint-angle anitform joint2-max-limit) 

(node-list anitform (list (list 0 0 0 1) (list 0 0 0 1) 

(list link21ength 0 0 1))) ; for mdh 

; (list (- link21ength) 0 0 1))) ; for dh 

(polygon-list anitform '((1 2))))) 
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(defclass link3 (rotary-link) 

((twist-angle initform 0) 

(link-length : initform link31ength) 

(inboard-joint-angle lintfonn joint3-init ) 
(inboard-joint-displacement anitform 0) 

(nnn-joint-angle anitform joint3-min-limit) 

(max -joint-angle anitform joint3-inax-liimt) 

(node-list initform (list (list 0 0 0 1) (list 0 0 0 1) 

(list link31ength 0 0 1))) for mdh 
(list (- link31ength) 0 0 1))) . for dh 
{polygon-list anitform '((1 2))))) 

for dh 

(defmethod update-A-matnx ((link link)) 

(with-slots (twist-angle link-length inboard-joint-angic 
inboard-joint-displacement A-matnx) link 
(setf A-matnx 

(dh-matnx (cos inboard-joint-angle) (sin inboard-joint-angle) 
(cos twist -angle) (sin twist -angle) 
link-length inboard-joint-displacement)))) 

, added for mdh 

(defmethod update-T-matnx ((link link)) 

(with-slots (twist-angle-i-1 link-length-i-1 inboard-joint-angle 
inboard-joint-displacement T-matrix) link 
(setf T-matrix 

(mdh-matnx (cos inboard-joint-angle) (sin inboard-joint-angle) 
(cos twist-angle-i-1) (sin twist-angle-i-1) 
link-length-i- 1 inboard-joint-displacement)))) 

(defmethod rotate ((link link) angle) 

(setf (inboard-joint-angle link) angle) 

(update-T-matrix link) 

(setf (H-matrix link) (matrix-multiply (H-matrix (inboard-link link)) 
(T-matrix link))) 

(transform-node-list link)) 

(defmethod rotate-link ((link link) angle) 

(cond ((> angle (max -joint-angle link)) 

(rotate link (max -joint-angle link)) 

(setf (motion-limit-flag link) t)) 

((< angle (min-joint-angle link)) 

(rotate link (min-joint-angle link)) 

(setf (motion-limit-flag link) t)) 

(t (rotate link angle) (setf (motion-limit-flag link) nil)))) 
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"aqua.cl" 



; requires STROBE-CAMERA. CL 
; requires AQUA-LEG. CL 

(defclass aquarobot-bodv (ngid-body) 

((mass anitform aqua-body-mass) 

(moments-of-inertia :imtform aqua-body-inertia) 

(node-list 

initform (list (list 0 0 0 1) 

(list (* (cos legl -angle) linkOlength) 

(* (sin legl -angle) linkOlength) 0 1) 

(list (* (cos leg2-angle) linkOlength) 

(* (sin legl -angle) linkOlength) 0 1) 

(list (* (cos leg3-angle) linkOlength) 

(* (sin leg3-angle) linkOlength) 0 1) 

(list (* (cos leg4-angle) linkOlength) 

(* (sin leg4-angle) linkOlength) 0 1) 

(list (* (cos leg5-angle) linkOlength) 

(* (sin leg5-angle) linkOlength) 0 1) 

(list (* (cos leg6-angle) linkOlength) 

(* (sin leg6-angle) linkOlength) 0 1) 

(list linkOlength 0 (- flag-length) 1))) 

(polygon-list 

anitform '((1 2 3 4 5 6) (1 7))))) 

(defclass aquarobot () 

((body 

anitform (make- in stance 'aquarobot-bodv) 

.accessor body) 

(legl 

anitform (make-instance 'aqua-leg : leg-attachment-angle legl -angle) 
: accessor legl) 

(leg2 

anitform (make-instance 'aqua-leg : leg-attachment-angle leg2-angle) 
accessor leg2) 

(leg3 

initform (make-instance 'aqua-leg : leg -attachment-angle leg3-angle) 
: accessor leg3) 

(leg4 

anitform (make-instance 'aqua-leg : leg-attachment-angle leg4-angle) 
: accessor leg4) 

(leg5 

anitform (make-instance 'aqua-leg : leg-attachment-angle leg5-angle) 
: accessor leg5) 

(leg6 

anitform (make-instance 'aqua-leg .leg-attachment-angle leg6-angle) 
:accessor leg6))) 
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(dcfmethod world-to-aqua ((aqua aquarobot) xyz-pos) 
(world-to-body (body aqua) xyz-pos)) 

(defmethod aqua-to-world ((aqua aquarobot) xyz-pos) 
(body-to-\vorld (body aqua) xyz-pos)) 

.(defmethod initialize ((aqua aquarobot)) 

(self (H-inatrix (body aqua)) 

(homogeneous-transform azimuth-imt ele\ation-init roll-imt 
x-init y-init z-imt)) 

(transform-node-list (body aqua)) 

(update-position (body aqua)) 

(setf (forces-and-torques (body aqua)) '(() 0 0 0 0 ())) 

(self (acceleration (body aqua)) ’(() 0 0 0 0 ())) 

(setf (velocity (body aqua)) '(() 0 0 0 0 0)) 

(start-timer (body aqua)) 

(imtialize-leg (legl aqua) (body aqua)) 

(imtialize-leg (leg2 aqua) (body aqua)) 

(initialize-leg (leg3 aqua) (body aqua)) 

(initialize-leg (leg4 aqua) (body aqua)) 

(imtialize-leg (leg 5 aqua) (body aqua)) 

(imtialize-leg (legb aqua) (body aqua))) 

(defun aqua-picture () 

(setf aqua- 1 (make-instance ’aquarobot)) 

(initialize aqua-1) 

(move-incremental aqua-1 null-move-list);sets "prev-foot-pos". 
(setf camera-1 (make-instance ’strobe-camera)) 

(take-picture camera- 1 aqua-1)) 

(defmethod take-picture ((camera strobe-camera) (aqua aquarobot)) 
(take-picture camera (body aqua)) 

(take-picture camera (legl aqua)) 

(take-picture camera (leg2 aqua)) 

(take-picture camera (leg3 aqua)) 

(take-picture camera (leg4 aqua)) 

(take-picture camera (leg5 aqua)) 

(take-picture camera (leg6 aqua))) 

(defun new-picture 0 
(erase-camera-window camera- 1) 

(take-picture camera-1 aqua-1)) 



107 



(defmethod move-incremental ((aqua aquarobot) increment-list) 
(move-incremental (body aqua) (first increment-list)) 
(move-incremental (legl aqua) (second increment-list)) 
(move-incremental (leg2 aqua) (third increment-list)) 
(move-incremental (leg3 aqua) (fourth increment-list)) 
(move-incremental (leg4 aqua) (fifth increment-list)) 
(move-incremental (leg5 aqua) (sixth increment-list)) 
(move-incremental (leg6 aqua) (seventh increment-list))) 

(defconstant null-move-list '((0 0 0 0 0 0) (0 0 0) (0 0 0) (0 0 0) 

(0 0 0 ) (0 0 0 ) (0 0 0 ))) 

(defmethod feasible-movep ((aqua aquarobot) allowable-sinkage 
allowable-slippage) 

(and (feasible-movep (legl aqua) allowable-sinkage allowable-slippage) 
(feasible-movep (leg2 aqua) allowable-sinkage allowable-slippage) 
(feasible-movep (leg3 aqua) allowable-sinkage allowable-slippage) 
(feasible-movep (leg4 aqua) allowable-sinkage allowable-slippage) 
(feasible-movep (leg5 aqua) allowable-sinkage allowable-slippage) 
(feasible-movep (leg6 aqua) allowable-sinkage allowable-slippage))) 

(defun restart-aqua () 

(initialize aqua-1) 

(move-incremental aqua-1 null-move-list);sets "prev-foot-pos M . 
(new-picture)) 

-.replace some rigid-body functions: 

(defmethod start-timer ((body aquarobot-bodv)) 

(setf (current-time body) 0)) 

(defmethod get-delta-t ((body aquarobot-bodv)) 

(let* ((delta-t *dt*) 

(new-time (+ (current-time body) delta-t))) 

(setf (current-time body) new-time) 
delta-t)) 

(defmethod update-aquarobot ((aqua aquarobot)) ;Euler integration. 

(let* ((body (body aqua)) 

(delta-t (get-delta-t body))) 

(update-acceleration body) 

(update-velocity body delta-t) 

(update-H-matrix body delta-t) 

(transform-node-list body) 

(update-position body) 

(update-forces-and-torques aqua))) .updates positions of legs 
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; "aqua-Ieg.cl" 



; requires AQUA. CL 
; requires AQUA-LINK CL 
, requires STROBE-CAMERA. CL 

(defclass aqua-leg () 
((leg-attachment-angle 
imtarg leg-attachment-angle 
accessor leg-attachment-angle) 
(luikO 

initform (make-instance 'linkO) 
accessor linkO) 

(link 1 

initform (make-instance 'link 1 ) 
accessor link 1 ) 

(link2 

initform (make-instance 'link2) 
accessor link2) 

(link3 

initform (make-instance 'link3) 
accessor link3) 
(motion-complete-flag 
initform ml 

accessor motion-complete-flag) 
(previous-foot-position 
:initform nil 

:accessor previous-foot-position) 
(current-foot-position 
initform nil 

accessor current-foot-position) 
(foot-contact 
initform ml 

accessor foot-contact))) 
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(defmethod mitialize-leg ((leg aqua-leg) (body aquarobot-body)) 
(setf (foot-contact leg) nil) 

(setf (inboard-link (linkO leg)) body) 

(setf (inboard-link (link 1 leg)) (linkO leg)) 

(setf (inboard-link (link2 leg)) (linkl leg)) 

(setf (inboard-link (link3 leg)) (link2 leg)) 

; added for mdh 

(setf (tvvist-angle-i-1 (linkO leg)) 0) 

(setf (twist-angle-i-1 (linkl leg)) (twist-angle (linkO leg))) 

(self (twist-angle-i-1 (link2 leg)) (twist-angle (linkl leg))) 

(setf (twist-angle-i-1 (link3 leg)) (twist-angle (link2 leg))) 

(setf ( link-length-i- 1 (linkO leg)) 0) 

(setf ( link-length-i- 1 (linkl leg)) (link-length (linkO leg))) 

(setf ( link-length-i- 1 (link2 leg)) (link-length (linkl leg))) 

(setf ( link-length-i- 1 (link3 leg)) (link-length (link2 leg))) 
(set-default-angles leg)) 

(defmethod set-default-angles ((leg aqua-leg)) 

(rotate-link (linkO leg) (leg-attachment-angle leg)) 

(rotate-link (linkl leg) jointl-init) 

(rotate-link (link2 leg) joint2-init) 

(rotate-link (link3 leg) joint3-imt) 

(setf (previous-foot-position leg) nil) 

(setf (current-foot-position leg) 

(near 3 (third (transformed-node-list (link3 leg)))))) ; for mdh 

(defmethod set-angles ((leg aqua-leg) angle-list) 

(rotate-link (linkO leg) (leg-attachment-angle leg)) 

(rotate-link (linkl leg) (car angle-list)) 

(rotate-link (link2 leg) (cadr angle-list)) 

(rotate-link (link3 leg) (caddr angle-list))) 

(defmethod take-picture ((camera strobe -camera) (leg aqua-leg)) 
(take-picture camera (linkl leg)) 

(take-picture camera (link2 leg)) 

(take-picture camera (link3 leg))) 
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(defmethod move-incremental ((leg aqua-leg) increment-list) 

(rotate-link (linkO leg) (leg-attachment-angle leg)) 

(rotate-link (link 1 leg) 

(+ (first increment-list) (inboard-joint-angle (lmkl leg)))) 

(rotate-link (link2 leg) 

(+ (second increment-list) (inboard-joint-angle (link2 leg)))) 
(rotate-link (link3 leg) 

(+ (third increment-list) (inboard-joint-angle (link3 leg)))) 

(setf (previous-foot-position leg) (current-foot-position leg)) 

(setf (current-foot-position leg) 

(near 3 (third (transformed-node-list (link3 leg))))) ; for indh 
(near 3 (first (transforined-node-list (link3 leg))))) ; for dh 
(setf (motion-complete-flag leg) (not (or (motion-hmit-flag (link 1 leg)) 
(motion-limit-flag (hnk2 leg)) (motion-limit-flag (link3 leg)))))) 

(defmethod feasible-movep ((leg aqua-leg) allovvable-sinkage allowable-slippage) 
(and (<= (third (current-foot-position leg)) allowable-sinkage) 

(or (minusp (third (current-foot-position leg))) 

(minusp (third (previous-foot-position leg))) 

(<= (vector-length (vector-slippage leg)) allowable-slippage)))) 

(defmethod vector-slippage ((leg aqua-leg)) 

(vector-subtract (rest (reverse (previous-foot-position leg))) 

(rest (reverse (current-foot-position leg))))) 

(defmethod current-joint-angles ((leg aqua-leg)) 

(list (inboard-joint-angle (hnkl leg)) 

(inboard-joint-angle (link2 leg)) 

(inboard-joint-angle (link3 leg)))) 
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; "aqua-inverse-kinematics. cl" 



; load after AQUA-LEG.CL 
load after AQUA-DATA. CL 

(defconstant L2sqr (sqr link2 length)) 

(defconstant L3sqr (sqr link31ength)) 

; assumptions: dh coord system for linkO of respective leg 
; origin at joint 1, 

x-axis directed away from center of body, 
z-axis aligned w ith body z-axis; 

; foot-position = ’(x y z). 

(defun theta 1 (foot-position) 

(if (< (car foot-position) 0) 

(atan2 (- (car foot-position)) (- (cadr foot-position))) 

(atan2 (car foot-position) (cadr foot-position)))) 

, assumptions: dh coord system for link 1 of respective leg: 
origin at joint2, 

x-axis directed away from joint 1 . 
z-axis aligned with body z-axis; 
foot-position = '(x v z); 
hyp = distance from joint2 to foot, 

(defun theta2 (foot-position hyp hvp-sqr) 

(- (acos (/ (+ L2sqr hvp-sqr (- L3sqr)) (* 2 link21ength hyp))) 

(if (< (car foot-position) 0) 

(- pi (asin (/ (caddr foot-position) hyp))) 

(asin (/ (caddr foot-position) hyp))))) 

; assumptions: same as for theta2. 

(defun theta3 (foot-position hvp-sqr) 

(- (acos (/ (+ L2sqr L3sqr (- hvp-sqr)) (* 2 link21ength link3 length))) pi)) 



, returns foot position with respect to joint 1 in linkO coord. 

(defmethod foot-jointl/linkOcoord ((leg aqua-leg) foot-pos) 
(vector-subtract (world-to-body (linkO leg) foot-pos) 

(list linkOlength 0 0))) 

; returns foot position with respect to joint 2 in linkl coord. 

; given foot-jointl/linkOcoord. 

(defim foot-joint2/linkl coord (foot-pos) 

(list (- (sqrt (+ (sqr (car foot-pos)) (sqr (cadr foot-pos)))) linkl length) 
0 (caddr foot-pos))) 
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, returns list of joint angles required for given (world coord) foot position 
(defmethod aqua-mv-kin ((leg aqua-leg) foot-position) 

(let* ((pos() (foot-joint l/hnkOcoord leg foot-position)) 

(posl (foot-joint2/link 1 coord pos())) 

(hvp-sqr (+ (sqr (car posl)) (sqr (caddr posl)))) 

(hyp (sqrt hvp-sqr))) 

(list (theta 1 posO) 

(thcta2 posl hyp hvp-sqr) 

(theta3 posl hvp-sqr)))) 



; "nqua-jacobiaii.cl" 



(defmethod jacobian ((leg aqua-leg)) 

(let* ((TO 1 (+ (leg-attachment-angle leg) 

(inboard-joint-angle (link 1 leg)))) 

(SOI (sin TO 1)) (C01 (cos TO 1)) 

(T2 (inboard-joint-angle (link2 leg))) 

(S2 (sin T2)) (C2 (cos T2)^ 

(T23 (+T2 (inboard-joint-angle (link3 leg)))) 

(S23 (sin T23)) (C23 (cos T23)) 

(LI link 1 length) (L2 Iink21ength) (L3 I mk3 length)) 

(list (list (- (* (+ LI (* L2 C2) (* L3 C23)) SOI)) 

(-(*(+(*L2 S2)(*L3 S23 )) CO 1 )) 

(-(* L3 C01 S23))) 

(list (* (+ LI (* L2 C2) (* L3 C23)) C01) 

(-(*(+(*L2 S2)(* L3 S23)) SOI)) 

(-(* L3 SOI S23))) 

(list 0 

(- (+(* L2 C2)(*L3 C23))) 

(- (* L3 C23)))))) 

(defmethod inverse-jacobian ((leg aqua-leg)) 

(matrix-inverse (jacobian leg))) 

(defmethod foot-to-joint-rates ((leg aqua-leg) dX dY dZ) 
(post-multiply (inverse-jacobian leg) (list dX dY dZ))) 

(defmethod joint-to-foot-rates ((leg aqua-leg) dthetal dtheta2 dtheta3) 
(post-multiply (jacobian leg) (list dthetal dtheta2 dtheta3))) 



113 



; "aquarobot-update-forces-and-torques.cl" 



; load after AQUA-DATA. CL 
; load after AQUA. CL 
; load after AQUA-LEG. CL 

(defmethod update-forces-and-torques ((aqua aquarobot)) 

(seif (forces-and-torques (body aqua)) '(0 0 0 0 0 0)) .clear last cycle, 
(add-leg-forces-and-torques (Leg 1 aqua)) 

(add-ieg-forces-and-torques (leg2 aqua)) 

(add-leg-forces-and-torques (leg3 aqua)) 

(add-leg-forces-and-torques (leg4 aqua)) 

(add-leg-forces-and-torques (ieg5 aqua)) 

(add-ieg-forces-and-torques (leg6 aqua))) 

(defmethod add-leg-forces-and-torques ((leg aqua-leg)) 

(if (or (foot-contact leg) (new-contact leg)) 

(let* ((body (inboard-link (linkO leg))) 

(joint-angles (aqua-inv-kin leg (current-foot-position leg)))) 
(set-angles leg joint-angles) 

(let* ((r (world-to-body body (current-foot-position leg))) 
(omega (cdddr (velocity body))) 

(foot-velocity ; in body coordinates 
(vector-add 

(scalar-multipiy -1 (near 3 (velocity body))) 
(cross-product r omega))) 

(torques (vector-add 
(mapear '* spring-constants 
(vector-subtract joint-angles default-angles)) 

(mapear '* spnng-damping-constants 
(post-multiply (inverse-jacobian leg) foot-velocity)))) 
(resultant-force 
(scalar-multipiy 
-1 (post-multiply 

(matrix-inverse (transpose (Jacobian leg))) 
torques)))) 

(if (still-in-contact leg resultant-force body) 
(add-forces-and-torques-to-body 
body r resultant-force)))))) 

(defmethod add-forces-and-torques-to-body ((body aquarobot-body) r f) 
(let ((torques (cross-product r f))) 

(setf (forces-and-torques body) 

(vector-add (forces-and-torques body) 

(append f torques))))) 
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.update joint angles and foot position detect foot hitting ground 
(defmethod new-contact ((leg aqua-leg)) 

(move-incremental leg '(0 0 ())) 

(if (> (third (current-foot-position leg)) 0) 

(setf (third (current-foot-position leg)) 0 
(foot-contact leg) t) 
ml)) 

.detect loss of contact, (positive/down z component in world coord) 

.side effect of resetmg leg to default state when nil is returned 
(defmethod still-in-contact ((leg aqua-leg) force/bod\-v\z 
(body aquarobot-body)) 

(let ((force/world-\\z (vector-subtract (bodv-to-uorld bod\ force/body-vyz) 
(location bod\ )))) 

(if (> (third force/world-.vvz) 0) 

(and (set-default-angles leg) (setf (foot-contact leg) nil)) 
t))) 
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APPENDIX D 



OPERATING INSTRUCTIONS 



Call "droptest" with zero to four arguments. 

First arg Spring Constant (2.. 15, default 5) 

Second arg Damper Constant(0.5.. 15, default 5) 

Third arg Drop Height (0.. 100, default 0) cm 

Fourth arg Update Time Increment (10. .50, default 50) ms 



SOURCE CODE FILES 



//file "droptest.c” 



/* 

/* droptest.c 
/* 

/* performer Aquarobot model with "spring" joints. 
/* 

/* */ 



include <stdlib.h> 

^include <stdio.h> 

^include <string.h> 

^include <math.h> 

^include <gl/device.h> 

/* performer */ 
include "pf.h" 

/* performer aqua-robot object constructor */ 

^include "pf_aqua.h" 

/* physical aqua-robot object constructor and controls */ 
^include "aqua.H" 

static void OpenPipeline (pfPipe *p); 
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void 

main (int argc, char *argv[]) 

t 

\ 

pfPipe *p; 
pfChannel *chan, 
pfScene *scene, 
pfDCS *robot_position 

pfGroup *aqua_robot. /* graphics object (performer) */ 

ptDCS *JointDCS[6|[4|, 

aquarobot robot, /* plnsical object */ 

II defaults for args 
float spring = SPRING_K, 
tloat damp = SPRING D, 
float height = 0 Of, 

float step = 0.05f, // default ~ real time 
If process args 

if (argc > 1 ) { // spring constant 
// first arg -15,000,000 <= spring <= -2,000,000 
spring = fabs((float)(atoi(argv[ 1 1))); 
if (spring < 2 Of) 
spring = 2 Of; 
else if (spring > 15. Of) 
spring = 15. Of; 
spring *= -1000000; 

} 

if (argc > 2) { // spring damping constant 
// second arg: -1,500,000 <= damp <= - 50,000 
damp = fabs((float)(atoi(arg\'[2]))); 
if (damp < 0.50 
damp = 0.5f; 
else if (damp > 15.00 
damp = 15. Of; 
damp *= -100000; 

} 

if (argc > 3) { 

// third arg: 0 <= drop height <= 100 
height = fabs((float)(atoi(arg\[3]))); 
if (height > 100.00 
height = 100. Of; 

} 

if (argc > 4) { 

// fourth arg: 10ms <= integration time step <= 50ms 
step = fabs((float)(atoi(arg\[4])))/1000.0f; 
if (step < 0.010 
step = 0.0 If; 
else if (step > 0.050 
step = 0.05f; 

} 
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/* 1. initialize Performer */ 
pflnit(); 

/* 2. configure MP mode and start parallel processes */ 
pfConfigO; 

/* 3. load scene database */ 
scene = plNewScene(); 

robot_position = pfNewDCS(); 
pfAddChild( scene, robot_position); 
aqua_ ro bot = MakeAquaRobot(JointDCS); 
pfAddChild(robot_position, aqua_robot); 

/* 5. configure and open full-screen pipeline */ 
p = pfGetPipe(O); 

pflnitPipe(p, OpenPipeline); /* pfInitPipe(p, NULL); */ 

/* set frames per second ( if step = .05 sec, then ~ real time) */ 
pfFrameRate( 20 . Of) ; 

/* 6. get and configure channel */ 
chan = pfNewChan(p); 
pfChanScene(chan, scene); 
pfChanNearFar(chan, 0. If, 1000. Of); 
pfChanFOV(chan, 45. Of, -l.Of); 

/* zero clock (not really needed) */ 
pfinitClockO; 

/* initialize robot *1 

robot. initialize(spring, damp, height); 

updateJointDCS(robot, JointDCS); 

/* set up view position */ 
pfCoord view; 

pfSetVec3(view.hpr, O.Of, 30. Of, 180. Of); 
pfSetVec3(view.xyz, O.Of, -500. Of, -350. Of); 
pfChanView(chan, view.xyz, view.hpr); 
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/* 7. create rendenng loop */ 

/* simulate for 30 seconds */ 
int t = 0, 

while (t < 600) // - 20 frames per second 

r 

i 

/* Transfer robot data to graphics object * 
pfDCSMatrix(robot_position, robot body H matrix); /* body 
updatejointDCS(robot, JomtDCS), * joints * 

/* Go to sleep till next frame time */ 
pfSyncO; t++, 

/* initiate cull/drau for this frame * 
ptFrameO, 

pfDrawChanStats(chan); 

/* Move robot to new position */ 
robot. update_aquarobot(step), 

} 

/* 8. terminate parallel processes and exit */ 

pfExit(), 

exit(0); 



/* 

* OpeiiPipeline() - create a pipeline: setup the window system. 

* the IRIS GL, and IRIS Performer. This procedure is executed in 

* the draw process (when there is a separate draw process). 

*/ 



static void 

OpenPipeline (pfPipe *p) 

i 

\ 

pfLight *Sun; 

/* negotiate with window-manager */ 
foreground(); 
prefposition(0, 600, 0,600); 
winopenf’Aqua Drop'’); 

/* negotiate with GL */ 
pfinitGfx(p), 

/* create a light source */ 

Sun = pfNewLight(pfGetSharedArenaO); 
pfLightPos(Sun, O.Of, O.Of, 1 .Of, 0.00; 

/* create a default lighting model */ 

pfApplyLModel(pfNewLModel(pfGetSharedAiena())); 

pfLightOn(Sun); 

} 
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//file "pf_aqua.h M 



/* 

* pf aqua.h 

* 

* call "MakeAquaRobot" to make AquaRobot performer object 

* 

* JointDCS[i](j] points to pfDCS for leg i, joint j. 

* where j = 0 is the shoulder joint, andj = 3 attaches the foot 

* 

*/ 

^include "pf.h" 
pfGroup* 

MakeAquaRobot(pfDCS ^ JointDCS [6 J [4 ] ). 



//file "pf_aqua.c" 



/* 

* pf_aqua.c 

* 

* call "MakeAquaRobot" to make AquaRobot performer object. 

* 

*/ 



#include "pf_aqua.h" 

^include "aquaJink.H" 

/* polygon data for aquarobot */ 
^include "polybody.h" 

^include "polyshoulder. h" 

^include "polyupperleg.h" 

#include "polylowerleg.h" 

^include "polyfoot, h" 

/* geostate for multiple parts */ 
static pfGeoState *robotyellow_gstate; 
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pfGeoSet* 

MakeBodyGSet(void) 

/ 

i 

pfGeoSet *gset; 
void *arena, 
pfMatenal *mtl; 

arena = pfGetSharedArena(); 
gset = pfNewGSet(arena); 

/* set the coordinate and normal arrays */ 

pfGSetAttr(gset. PFGS_COORD3, PFGS_PER_VERTEX, bodycoords, NULL); 
pfGSetAttr(gset. PFGS_NORMAL3, PFGS_PER_PRIM, bodynorms, NULL); 

pfGSetPrimType(gset. PFGS_QUADS); 
pfGSetNumPnms(gset, 94); 

/* set up geostate for "robotyellow" matenal */ 
robotyeIIo\v_gstate = pfNewGState(arena); 
mtl = pfNevvMtl(arena); 

pfMtlColorfmtl, PFMTL_ AMBIENT, 0.2f, 0.2f. 0.00: 
pfMtIColor(mtl, PFMTLJD1FFUSE, 1 .Of, 1. Of, 0.00; 
pfMtlCoIor(mtl, PFMTL_EMISSION, O.Of, O.Of, 0.00; 
pfMtlColor(mtl, PFMTL_SPECULAR. O.Of, O.Of, 0.00; 
pfMtlAlpha(mtl, 1.0); 

pfGStateAttr(robotyello\v_gstate, PFSTATE FRONTMTL, mtl); 
pfGSetGState(gset, robotyellovv_gstate); 

return gset; 



pfGeoSet* 

MakeLinkl GSet(void) 

{ 

pfGeoSet *gset; 
void *arena; 

arena = pfGetSharedArena(); 
gset = pfNewGSet(arena); 

pfGSetAtU(gset, PFGS_COORD3, PFGS_PER_VERTEX, linklcoords, NULL); 
pfGSetAttr(gset, PFGSNORMAL3, PFGS_PER_PRIM, linklnorms, NULL); 

pfGSetPrimType(gseL PFGS_QUADS); 
pfGSetNumPrims(gseL 42); 

pfGSetGState(gset, robotyellow_gstate); 

return gset; 

} 
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pfGeoSet* 

MakeLink2GSet(void) 

{ 

pfGeoSet *gset, 
void *arena, 

arena = pfGetSharedArenaO; 
gset = plNewGSet(arena); 

pfGSetAttr(gset, PFGS_COORD3. PFGS_PER_VERTEX. link2coords. NULL). 
pfGSetAttr(gset. PFGS_NORMAL3, PFGS_PER_PR1M. link2norms. NULL); 

pfGSetPrimTvpe(gset. PFGS_QUADS); 
pfGSetNuinPnms(gset, 91), 

pfGSetGState(gset, robotyello\v_gstate); 

return gset. 



pfGeoSet* 

MakeLink3GSet(void) 

{ 

pfGeoSet *gset; 
void *arena; 

arena = pfGetSharedArenaO; 
gset = pfNevvGSet(arena); 

pfGSetAttr(gset, PFGS_COORD3, PFGS_PER_VERTEX, link3coords, NULL), 
pfGSetAttr(gset, PFGSNORMAL3, PFGS_PER_PRJM, link3norms, NULL); 

pfGSetPrimTvpe(gset, PFGS_QUADS), 
pfGSetNumPnms(gset, 103); 

pfGSetGState(gset, robotyello\v_gstate); 

return gset; 

} 
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pfGeoSet* 

MakeFootGSet(void) 

i 

pfGeoSet *gset; 
void ’“arena; 

arena = pfGetSharedArena(); 
gset = pfNewGSet(arena); 

pfGSetAttr(gset. PFGSCOORD3, PFGS_PER_ VERTEX, footcoords. NULL); 
pfGSetAttr(gset, PFGS_NORMAL3, PFGS_PER_PRL\i footnorms, NULL); 

pfGSetPrimTvpe(gset, PFGS_TRISTRIPS); 
pfGSetNumPrims(gset, 49); 
pfGSetPnmLengths(gset, footstnplengths); 

pfGSetGState(gset, robotyello\v_gstate); 

return gset; 



pfGeoSet* 

MakeShaftGSet(void) 

{ 

pfGeoSet *gset, 

void *arena; 

pfGeoState *robotgray_gstate; 

pfMaterial *mtl; 

arena = pfGetSharedArena(); 

gset = pfNewGSet( arena); 

pfGSetAttr(gset, PFGS _COORD3, PFGS_PER_VERTEX, shaftcoords, NULL); 
pfGSetAttr(gset, PFGS_NORMAL3, PFGS_PER_PRIM, shaftnorms, NULL); 

pfGSetPrimType(gset, PFGS_QUADS); 
pfGSetNumPrims(gset, 20); 

/* set up material */ 

robotgraygstate = pfNewGState(arena); 
mtl = pfNewMtl(arena); 

pfMtlColor(mtl, PFMTL_ AMBIENT, 0.1, 0.1, 0.1); 
pfMtlColor(mtl, PFMTL_DIFFUSE, 0.2, 0.2, 0.2); 
pfMtlColor(mtl, PFMTL_EMISSION, 0.0, 0.0, 0.0); 
pfMtlColor(mtl, PFMTL_SPECULAR, 0.0, 0.0, 0.0); 
pfMtlAlpha(mtl, 1.0); 

pfGStateAttr(robotgray_gstate, PFSTATE_FRONTMTL, mtl); 
pfGSetGState(gset, robotgray_gstate); 

return gset; 

} 
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pfGroup* 

MakeAquaRobot( pfDC S *JointDCS[6||4]) 

{ 

pfSCS *LegAttachSCS|6], 

*LinklSCS[6], *Link2SCS[6], *Link3SCS|6|, 
*FootSCS[6], 

pfMatrix rot_mat. trans_mat, 

pfGroup * AquaRobotGroup, *Bod\Group[6|, *LegGroup|6|; 
pfGcodc *geode_body, 

*geode_linkl. *geode_link2. *geode_hnk\ 
*geode_shaft, *geode_foot. 
mt i; /* loop counter */ 

/* make geodes */ 
geode_body = pfNeuGeodeO; 
pfAddGSet(geode_body, MakeBodyGSctO); 

geodejink 1 = pfNe\\Geode(); 
pfAddGSet(geode_link 1 , MakeLink 1 GSetQ), 

geode _link2 = pfNe\\Geode(); 
pfAddGSet(geode_link2, MakcLink2GSet()); 

geode_link3 = pfNeuGeodeO; 
pfAddGSet(geode_lrnk3, MakeLink3 GSetQ), 

geode_foot = pfNe\vGeode(); 
pfAddGSet(geode_foot, MakeFootGSetO); 

geode_shaft = pfNe\vGeode(); 
pfAddGSet(geode_shaft, MakeShaftGSetO); 

/* Make Parent Group *f 
AquaRobotGroup = pfNewGroupO; 

/* Add Structure (6 segments) */ 
for(i = 0; i < 6; i++) 

{ 

/* rotate to segment */ 

pfMakeRotMat(rot_mat, i*60.0, 0.0, 0.0, 1.0); 
LegAttachSCS[i] = pfNewSCS(rot_mat); 
pfAddChild( AquaRobotGroup, LegAttachSC S [ i ] ); 

/* add body slice */ 

BodyGroup[i] = pfNewGroupO; 
pfAddChild(LegAttachSCS[i], BodyGroup[i]), 
pfAddChild(BodyGroup[i), geode_body); 
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/* add leg */ 



/* link 1 */ 

pfMakeTransMat(trans_mat, LINKOLENGTH, 0.0, 0.0); 
Link 1 SCS[i] = pfNevvSCS(trans_mat); 
pfAddChild(BodyGroup[i], LinklSCS[i]); 
pfAddChild(LinklSCS[i], geode_shaft); 

JointDCS[i][0| = pfNevvDCSQ; 
pfAddChild(Link 1 SCS[i], JointDCS[i][0]); 
pfAddChild( JointDCS[i] [0], geodejink 1 ); 

/* link 2 */ 

pfMakeRotMat(rot_mat, -90.0, 1.0, 0.0, 0.0); 
pfMakeTransMat(trans_mat, LINK1LENGTH. 0.0. 0.0); 
pfPostMuItMat( rot_mat, trans_mat), 

Link2SCS[i] = pfNevvSCS(rotmat); 
pfAddChild(JointDCS[i][0], Link2SCS[i]); 
pfAddChiId(Link2SCS[i], geode_shaft); 

JointDCS[i][l] = pfNewDCSQ; 
pfAddChild(Link2SCS[i], JointDCS[i] [ 1 ]); 
pfAddChild(JointDCS[i] [ 1 ], geode Jink2); 

/* link 3 */ 

pfMakeTransMat(trans_mat, LrNK2LENGTH, 0.0, 0.0); 
Link3SCS[i] = pfNewSCS(trans_mat); 
pfAddChild(JointDCS[i] [ 1 ], Link3SCS[i]); 
pfAddChild(Link3SCS[i], geode_shaft); 

JointDCS[i][2] = pfNewDCS(); 
P fAddChild(Link3SCS[i], JointDCS[i][2]); 
pfAddChild(JointDCS[i][2], geode_link3); 

/* foot */ 

pfMakeTransMat(trans_mat, LINK3LENGTH, 0.0, 0.0); 
FootSCSfi] = pfNewSCS(trans_mat); 
P fAddChild(JointDCS[i][2], FootSCS[i]); 

JointDCS[i][3] = pfNewDCSQ; 
pfAddChild(FootSCS[i], JointDCS [i] [3 ]); 
pfAddChild(JointDCS[i][3], geode_foot); 

} 

return AquaRobotGroup; 

} 
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// file "aqua.h” 



//ifndcf_AQUA_H 
//define _AQUA_H 



//include <Performcr/pf h> 
#mclude "inisc H" 

//include "rigid body H" 
-include "aqua_leg H" 



ty pedef rigidbody aquarobotbody 



/* mass in Kg. */ 

^define AQUABODYMASS 500 Of 



/* (lx Iy Iz)-Kg-cm in principal axis coordinates. */ 

/* assumes solid cy lindrical body of constant density */ 

^define AQUA_BODY_HEIGHT 50.0f 
#define AQUABODYRADIUS 30 Of 
static pfVec3 aqua_body_inertia = { 

// Ix 

1 Of/4 Of * AQUABODYMASS * AQUA BODY RADIUS * AQUA_B0DY_RAD1US 
+ I. Of/12 Of * AQUA_BODY_MASS * AQUA BODY HEIGHT * AQUA BOD Y HE1GHT. 
// ly 

1.0f/4 Of * AQUABODYMASS * AQUABODYRADIUS * AQUA_BODY_RADIUS 
+ 1. Of/12. Of * AQUA_BODY_MASS * AQUA_BODY_HEIGHT * AQUA_BODY_HEIGHT. 

// Iz 

1. 0f/2. Of * AQUA BODY MASS * AQUA_BODY_RADIUS * AQUA_BQDY_RAD1US}; 



/* imtial position and orientation in world coordinates. */ 

#define AZIMUTH_INIT deg_to_rad(0.0f) 

#define ELEVATI0N_IN1T deg_to_rad(0.0f) 

//define ROLL_INIT deg_to_rad(0.00 
//define XJNITO Of 
//define YJNIT O.Of 

#define Z_INIT sinf(default_angles[ 1 ])*LINK2LENGTH - LINK3LENGTH 



/* leg attachment angles. */ 

degjo_rad(0.0f) 
dcg_to_rad(60.0f) 



//define LEG 1 ANGLE 
#define LEG2 ANGLE 
//define LEG3ANGLE 
//define LEG4 ANGLE 
//define LEG5ANGLE 
//define LEG6 ANGLE 



deg_to_rad(120.0f) 

deg_to_rad( 180.00 
deg_to_rad(240.()0 
dcg_to_rad( 3 < K)() E) 
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class aquarobot 
public: 

aquarobot_body body; 

//private: 

aqua_leg legl, leg2, leg3, leg4, leg5, leg6; 

private: 

void 

aqua robot :: i ni t Jo i n t_access( ) ; 
void 

update_forces_and_torques(); 

void 

updateJegsQ; 

public: 

// External access to joint angles for passing to performer model 
// This could be private if "updateJointDCS" were a fnend; 

// however, the class should not depend on needs of user, 
float* joint_matrix[6][4], 

public: 

aquarobot():body(AQUA_BODY_MASS, aqua_bodyJnertia), 
legl (LEG 1 ANGLE), 
leg2(LEG2 ANGLE), 
leg3(LEG3 ANGLE), 
leg4(LEG4 ANGLE), 
leg5(LEG5 ANGLE), 

leg6(LEG6 ANGLE) { init Joint_access(); } 



void 

initialize(float k = SPRING_K, float d = SPRING_D, float height = O.Of); 
void 

update_aquarobot(float dt = O.Of); 

// coordinate transformation routines 
void 

world_to_aqua(pfVec3 destination, pfVec3 source) 

{body. world_to_body(destination, source); } 



void 

aqua_to_world(pfVec3 destination, pfVec3 source) 
{body.body_to_world(destination, source); } 

}; 
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void 

updatejointDCS(aquarobot robot, pflDCS *JointDCS|6](4|); 
#endif 
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// File "aqua.c" 



^include "aqua.H" 

#include <math.h> 

/* user routines */ 

void 

aquarobot.:initialize(float k, float d, float height) 

{ 

body move(AZIMLTH_INIT, ELEVATIONJNIT, ROLLJNIT. 

X_INIT, Y_rNIT, -fabs(height)+Z_INTT); 
pfSetVec3(body.vel_trans, O.Of, O.Of, O.Of); 
pfCopyVec3(body vel_rot, body.vel_trans); 
pfCopyVec3(bodv.accel_trans, body.vel_trans); 
pfCopyVec3(body accel rot, body.veltrans); 
pfCopyVec3 (body. forces, body.vel_trans); 
pfCopyVec3(body.torques, body.vel_trans); 
body.start_timer(); 
legl init_leg(&bodv, k, d); 
leg2.init_leg(&body, k, d); 
leg3.imt_leg(&body, k, d); 
leg4 initJeg(&body, k, d); 
leg5. ini t_leg(&body, k, d); 
leg6.init_leg(&body, k, d); 
update_forces_and_torques(); 

} 

void aquarobot::update_aquarobot(float dt) 

{ 

float dt_ = dt; 
if (dt <= 0.0) 

dt_ = body.get_delta_t(); // default 

body.update_acceleration(); 

body.update_velocity(dt_); 

body.update_H_matrix(dt_); 

body.update_position(dt_); 

// bodv.update_velocity(dt_); 
updateJegsO; 

/* This is done last as it also updates leg positions: */ 

/* leg positions depend on "new” body position! */ 

update_forces_and_torques(); 

} 
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/* Internal Routines *t 



void 

aquarobot: imtjoint_access() 

{ 

// for use, see fn:"updatejointDCS" below 
joint_matrix[0|[0] =&legl linkl inboardjoint_angle, 
joint_matri.x[0|[ 1 ] = &legl.lmk2 inboard joint_angle. 
joint_matri.x|0|[2| = &legl link3.mboardjoint_angle, 
joint_matri.x[0|[3] = &legl link4 H_matri\[0][0], 

joint_matnx[ 1 1[0| = &leg2. linkl inboardjoint_angle, 
joint_matrix[ 1 ][ 1 ] = &leg2.1ink2. inboard jointangle. 
joint_matrix[l][2| = &leg2.1ink3 . inboard joint_angle, 
joint_matnx[l|[3] = &leg2.1ink4.H_matn.x[0][0], 

joint_matrix[2][0] = &leg3. linkl. inboardjointjingle, 
joint_matnx[2][l] = &leg3.1ink2. inboard Jointangle, 
joint_matrix[2][2| = &leg3.1ink vinboardjomt_angle. 
joint_matrix[2|[3] = &leg3.1ink4.H_matnx[0][0]; 

joint_matrix[3 ] [0| = &leg4. link 1 inboard jomt_angle, 
joint_matrix[3][l] = &leg4.1ink2.inboardjoint_angle; 
joint_matrix[3][2] = &leg4.1ink3 inboardjoint angle; 
joint_matrix[3][3] = &leg4.1ink4 H_matrix[0][0]; 

joint_matrix[4][0] = &leg5. linkl. inboard joint_angle, 
joint_matnx[4][l] = &leg5.1ink2.inboardJoint_angle, 
joint_matrix[4][2] = &leg5.1ink3.inboardjoint_angle; 
joint_matrix[4)[3] = &leg5.1ink4.H_matnx[0|[0], 

joint_matrix[5][0] = &leg6. linkl. inboardjoint_angle; 
joint_matrix[5 ) [ 1 ] = &leg6.iink2. inboard joint_angle; 
joint_matrix[5][2] = &leg6.1ink3.inboardjoint_angle, 
joint_matrix[5][3] = &leg6.1ink4.H_matnx[0][0]; 



v oid aquarobot: : update Jorces_andJorques() 

{ 

pfSetVec3 (body. forces, O.Of, O.Of, 0 Of); 
pfSetVec3 (body torques, O.Of, O.Of, 0.00: 
leg 1 .add Jeg_forces_and_torques(); 
leg2.addJeg_forces_and_torques(); 
leg3 .add Jeg Jorces_and_torques(); 
leg4 . add_leg_forces_and_torques() ; 
legS.add leg_forces_andjorques(); 
leg6 . add Jeg_forces_and_torques() ; 

} 
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void aquarobot::update_legs() 

{ 

legl.update_leg(); 
leg2.update_leg(); 
leg3.update_leg(); 
leg4 update_leg(); 
leg5.update_leg(), 

Ieg6 update_leg(); 

} 



/* joint angle transfer routine */ 

\oid 

updatejointDCS(aquarobot robot, pfDCS *JointDCS[6][4|) 

\ 

\ 

static pfMatnx m4 = { {0,0,0,0},{0, 0,0,0}, {0,0,0,0}, {0,0,0, l 
for(int i=0;i<6;i++) { 

// rotate first three joints 
for(int j=0;j<3;j++) { 
pfDCSRot(JointDCS[i] [j |, 

rad_to_deg(*robot.joint_matnx[i][j]), O.Of, O.Of); 

} 

// equiv to rot(0x,-90y,0z) * inverse(leg[i].link4.H_Matrix) 

// accomplishes DCS such that link(x-axis) || vvorld(z-axis) 

m4[0][0] = (robot .joint_matrix[i) [3 ])[2]; 

m4[0][l] = (robot.joint_matrix[i][31)[6]; 

m4[01[2] = (robot joint_matrix[i] [3])[ 10]; 

m4[l][0] = ( robot joint_matrix[i] [ 3 ] ) [ 1 ] ; 

m4[l][l) = (robot.joint_matrix[il[3])[5]; 

m4[l|[2| = (robot.joint_matrix[i][3])[9]; 

m4[2][0] = -(robot joint rnatrix[i][3 ])[0]; 

m4[2][l] = -(robot joint_matrix[i][3])[4]; 

m4[2)[2] = -(robot joint_matrix[i] [3 ])[8]; 

pfDCSMatrix(JointDCS[i][3], m4); 
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//file "aqua leg. h" 



#ifndef _AQUA_LEG_H 
^define _AQUA_LEG_H 

^include <Performer/pr h> 

^include "rigid bod> H" 

#include "aqua Jink. H" 

^include "nusc.H" 

f* initial (default) joint angles */ 
static pfVec3 default_angles = { 

/* 0 deg */ O.Of, 

/* 45 deg*/ 25. Of * P1_F / 180. Of, 

/* -135 deg */ -1 15. Of * P1_F / 180.00; 

/* joint spring constant, (default : 5,000,000 Kg-cm2/sec2 per radian) */ 

^define SPRING_K -5000000 Of 

/* joint spring damping constant, (default 500,000 Kg-cm2/sec2 per radian/sec) */ 
^define SPRING_D oOOOOO.Of 

/* AQUA LEG CLASS */ 

class aqua_leg 

{ 

public; 

float legattachmentangle; 

aqualinkO linkO; 

aqualinkl link 1 ; 

aqualink2 iink2; 

aqualink3 link3; 

aqualink4 link4, 

boolean motion_completeJlag; 

pfVec3 previous Joot_position; 

pfVec3 current Joot_position; 

boo 1 ean foot_co ntact; 

float spr_k; 

float spr_d; 

public: 

aqua_leg(float angle = O.Of): leg_attachment_angle(angle) 

{ 

motion_complete_flag = TRUE; 
foot_contact = FALSE; 

} 



void 

init_leg(rigid_bodv *bod\\ float k = SPRING K, float d = SPRING_D), 
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void 

set_default_angles(); 

void 

updateJegO; 

void 

update_foot_pos(); 

void 

set_angles(float joint 1, float joint2, float joint3); 
void 

set_angIes(pfYec3 angles) {set_angles(angles(0], angles[l|, angles[2)); } 
void 

jacobianipfMatrix J); 
void 

inverse Jacobian(pfMatrix J_inv); 
void 

joint_rates(pfVec3 rates); 
void 

FootPosFmJointl(pfVec3 foot_posJl, pfVec3 foot _pos_\vorld); 
void 

aqua_inv_kin(pfVec3 joint_angles, pfVec3 \vorld_foot_pos); 
void 

add_leg_forces_and_torques(); 

int 

ne\v_contact(); 

int 

sti 1 l_i n_co ntact(pfV ec3 leg_force_body ) ; 



#endif 
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// file "aqua_leg.c M 



^include <math h> 

^include "aquaJcg.H" 

/* AQUA-ROBOT INVERSE KINEMATICS ROUTINES V 

static float L2sqr = LINK2LENGTH * LINK2LENGTH; 
static float L3sqr = LINK3LENGTH * LINK 3 LENGTH; 

/* routines that return the joint angles lor a leg, given the foot position * 

tloat 

theta l(pfVec3 foot_pos) 

{ 

if (foot_pos[0| < 0.00 
return (atan2f(-foot_pos[ 1 1, -foot_pos[0])); 
else 

return (atan2f( foot_pos[l], foot_pos[0])), 

} 

float 

theta2(pfVec3 foot_pos, float hvp, float h\p_sqr) 

{ 

float temp = asinf(foot_pos(2]/h\p); 
if (foot_pos[0] < 0.00 temp = PI_F - temp; 

return (acosf((L2sqr + h\p_sqr - L3sqr) / ( 2 * LINK2LENGTH * hvp)) - temp ); 

} 



float 

theta3 (float hvp_sqr) 

{ 

return (acosf((L2sqr + L3sqr - hvp_sqr) / (2 * L1NK2LENGTH * LINK3LENGTH)) - PI_F) 

} 

/* supports theta2 and theta3 which require foot position with respect */ 

/ * to joint2 position. joint2 position depends on theta 1 */ 

void 

FootPosFmJoint2(pfVec3 destination, pfVec3 source) 

{ 

destination^] = sqrtf(source[0]*source[0] 4- source[l]*source[l|) 

- LINK1LENGTH; 
destination! 1 ] = O.Of; 
destination^] = source[2]; 

} 
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/* AQUA LEG CLASS V 



void 

aquaJeg::initJeg(rigid_body *body, float k, float d) 

{ 

spr_k = k; spr_d = d, 
foot_contact = 0; 
link!) inboardjink = body; 
link 1 inboardjink = &link(); 
link2. inboardjink = &linkl; 
link3. inboardjink = &link2; 
link4. inboard link = &link3; 
set_default_angles(); 



void 

aquajeg::set_default_angles() 

s 

\ 

setangl es(defaul tangl es) ; 
update_foot_pos(); 

pfCopyVec3 (previous Joot_position, current_foot_position); 



void 

aqua Jeg : update Jeg() 

r 

\ 

pfCopy Vec3 (previous Joot_position, current Joot_position); 
set_angles(linkl. inboard joint_angle, 
link2. inboard Joint_angle, 
link3. inboard Joint_angle); 
u pda te_foo t_po s() ; 

} 



void 

aqua_leg : : update_foot_pos() 

{ 

if (!foot_contact) { 

current Joot_position[0J = link4.H_matrix[3][0J; 
current Joot_position[ l ] = link4.H_matrix[3J[lJ; 
current Joot_position [2] = link4.H_matrix(3][2]; 

} 

} 
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void 

aqua leg: set_angles(float joint 1 Hoat joint2. float joint V) 

{ 

linkO rotate Jink(legjittachinent angle), 
link 1 rotate_link(joint 1 ); 
link2 rotate Jink (jomt2); 
link3 rotate link(joint3); 

// tins works for each leg in 2D, but world !D solution requires(0 0) 
// link4 rotate_lmk( -dcg to rad(90 00 - joint2 - joint!); 
hnk4 rotate link(0 00. 



void 

aquajeg jacobian(pfMatn\ J) 

t 

\ 

pfVec3 row; 

noat angle, SOI, COI, S2. C2. S23, C23; 

#define LI LINK I LENGTH 
^define L2 LINK2LENGTH 
#defme L3 LINK3 LENGTH 

angle = leg_atiachment_angle + link 1. inboard jointangle, 
SOI = sinf(angle), 

COI = cosf(angle). 

angle = link2.inboardjoini_angle, 

S2 = sinf(angle). 

C2 = cosf(angle); 

angle += link3.inboardJoint_angle; 

S23 = sinf(angle); 

C23 = cosf(angle); 

pfMakeldenlMat(J); 

pfSctVec3(row, -SOI * (LI + L2*C2 + L3*C23), 

-COI * (L2*S2 +L3*S23), 

-COI * L3 * S23); 
pf5etMatColVec3(J, 0, row); 
pfSetVec3(row. COI * (LI + L2*C2 + L3*C23), 

-SOI * (L2*S2 + L3*S23), 

-SOI * L3 * S23); 
pfSetMatColVec3(J. 1, row); 
pfSetVec3(row, O.Of, 

-L2*C2 - L3*C23, 

-L3 * C23); 

pfSetMatColVec3(J, 2, row); 

} 
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void 

aquajeg:: inverse jacobian(pfMatnx J_inv) 

{ 

pfMatnx J; 
jacobian(J); 
pfInvertMat(J_inv, J); 

} 

void 

aquajeg: :joint_rates(pfVec3 rates) 

/ 

\ 

pfMatnx J_inv; 
pfVec3 translates; 
pfVec3 omega; 
pfVec3 foot_r; 
pfVec3 rot_rates; 
pfVec3 foot_rates; 

inverse Jacobian(J_inv); 

pfScaleVec3(trans_rates, - 1 .Of, ((rigidbody *)linkO inboard link)->vel trans); 

pfCopvVec3(omega. ((ngid_body *)linkO. inboard link)->vel_rot); 

((rigid body JlinkO. inboard Jink)->\vorldjo_body (footer. current Joot_position); 
pfCrossVec3(rot_rates, omega, foot_r); 

pfSubVec3( foot_rates, trans_rates, rot_rates); 
post_mult(rates, JJnv, foot rates); 

} 

void 

aqua_leg::FootPosFmJointl(pfVec3 foot_posJl, pfVec3 foot_pos_\vorld) 

/ 

i 

linkO. world to_body(foot_pos J 1, foot_pos_world); 
foot_posjl[0] -= LINKOLENGTH; 

} 

void 

aquajeg: :aquaJnvJcin(pfVec3 joint_angles, pfVec3 vvorld_footjpos) 

{ 

pfVec3 footjointl, footjoint2; 
float hyp, hyp_sqr; 

FootPosF mJoint 1 (foot Joint 1 , world Joot_pos) ; 

FootPosFmJoint2(footJoint2, foot Joint 1); 
hyp = pfLengthVec3(footJoint2); 
hvp sqr = hyp * hyp; 

pfSetVec3(joint_angles, theta l(foot Joint 1), 

theta2(footJoint2, hyp, hyp sqr), 
theta3(hyp_sqr)); 
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void 

aqua Jeg add Jeg_forces_andjorques() 

{ 

if (foot_contact || new_contact()) 

{ 

pfVec3 angles, jointjorques, damp, forces, footjx>s, 
piMatnx \vork_matnx 1 , \\ork_matrrx2, 

aqua_inv_kin(angles, currentfootposition). 
set_angles(angles); 

/* get spring force of joints */ 
pfSubVec3(jomt_torques. angles, default_angles), 
pfScaleVec3(jomt torques, spr k, joint_torques), 

/* add damping */ 
joint_rates(damp), 
pfScaleVec3(damp, spr d, damp), 
pfAddVec3(joint_torques, joim_torques, damp). 

jacobi an(\vork_matnx 1 ); 
pfTransposeMat(\vork_inatri\2, work matnx 1 ); 
pfinvertMat(\vork_matrixl, \vork_matnx2); 
post_mult(forces, \vork_matnxl, joint_torques); 
pfScaleVec3(forces, -1.0, forces); 

if (still Jn_contact(forces)) 

{ 

((ngid_body *)link0. inboard Jink)->\vorld_toJ)od>(foot_pos, current_foot_position), 
((rigid_body *)link0. inboard Jink)->add_force_and_torques(foot_pos, forces); 

} 

} 



int 

aqua_leg iievv_contact() 

{ 

if (cunent_foot_position[2| > O.Of) 

{ 

current_foot_position[2] = O.Of; 
return (foot_contact = TRUE); 

} 

else 

return FALSE; 
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int 

aqua_leg: :still_in_contact(pfVec3 leg_force_body) 

{ 

pfVec3 leg_force_\vorld; 

((ngid_bodv *)link0.inboard link)->body_to_vvorld(leg_force_\vorld, leg_force_body); 
pfSubVec3(leg_force_\vorld, leg_force_\vorld. ((ngid_body *)link0.inboardjink)->location), 
if (leg_force_vvorld[2] > O.Of) 

{ 

set_default_angles(); 

return (foot_contact = FALSE), 

} 

else 

return TRUE, 
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// file "aqua_link.h" 



#ifndcf_AQUA_LINK_H_ 

^define _AQUA_LINK H_ 

^include <Performer/pf h> 

^include "rigid_body H" 

/* BASE CLASSES */ 

class link public rigid_body 

f 

l 

public: 

int motionjimit flag; 

float length_i_l; 

float twist ij, 

float inboard joint_angle; 

float inboard joint displacement, 

void* inboardjink; 

pfMatrix Tjnatrix, 



public: 

linkffloat mass = 1 Of, pfVec3 moments = MULL): ngid body (mass, moments)}} 
void updateJIJmatrix(); 
void rotate(float angle); 

}; 



class rotarv Jmk.public link 

{ 

public: 

float minjoint_angle; 
float max jointangle; 



public: 

rotary _link(float length = O.Of, 

float min angle = O.Of, 

float max_angle = O.Of, 

float twist = O.Of, 

float joint_angle = O.Of, 



float joint_displacement = O.Of, 



void* inboardjink = 0); 



void rotate_link(float angle); 



}; 
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/* MODIFIED DANEVIT-HARTENBERG LINK COORDINATE TRANSFORMATION MATRIX */ 



void 

mdh_matn.\(pfMatnx mdh, 

float cosrotate, float sinrotate, 
float costwist_i_l, float sint\vist_i_L 
float length_i_L float translate); 



/* AQUA-LINK CLASSES */ 

/* link lengths */ 

^define LINKOLENGTH 37.5f 
#define LINK1LENGTH 20.0f 
^define LINK2LENGTH 52. Of 
^define LINK3LENGTH 102.0f 
#define LINK4LENGTH 3 . Of 

/* joint limits */ 

#define JOINT 1 MIN deg_to_rad( -60. Of) 
^define JOINT 1 MAX deg_to_rad( 60. Of) 
^define JOINT2MIN deg_to_rad(-360.0f) 
^define JOINT2MAX deg_to_rad( 360.0f) 
^define JOINT3MIN deg_to_rad(-360.0f) 
^define JOINT3MAX deg_to_rad( 360.00 
^define JOINT4MIN deg_to_rad( -22.50 
^define JOINT4MAX deg_to_rad( 22.50 

class aqualink0:public rotarvjink 

{ 

public: 

aqualink0(); 

}; 



class aqualinkLpublic rotary__link 

{ 

public: 

aqualinkl(); 

}; 

class aqualink2:public rotaryjink 

{ 

public: 

aqualink20; 

}; 
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class aqualink3:public rotarvjink 

{ 

public: 

aqualink3(); 

* 

(> 



class aqualink4 : public rotary link 

{ 

public 

aqualink4(); 

i- 
f > 



#endif 



// file M aqua_link.c" 



#include <math.h> 

#include M aqua_link.H" 
include "misc.H" 

/* BASE CLASSES */ 

\oid 

link update_T_matrix() 

{ 

float sa = sinf(inboardjoint_angle); 
float ca = cosf(inboardjoint_angle); 
float st = sinf(twist_i_l), 
float ct = cosf(t\vist_i_l); 

mdh_matrix(T_rnatrix, ca, sa, ct, st, length_i_l. inboard joint displacement); 

} 



void 

link::rotate(float angle) 

{ 

inboard joint_angle = angle; 
update_T_matnx(); 

// multiplied in reverse order as they are stored as transposes. 
pfMultMat(H_matrix, T_matrix, ((ngidbody *)inboard_link)->H_matnx); 

} 

rotary link: :rotary_link(float length, 
float nun_angle, 
float max_angle, 
float twist, 
float jointangle, 
float joint ^displacement, 
void* inboard_link_) 

{ 

length _i_l = length; 

minJoint_angle = min_angle; 

maxJoint_angle = max_angle; 

twist_i_l = twist; 

inboard Joint_angle = joint_angle; 
inboardjoint jiisplacement = joint_displacement; 
inboardlink = inboard_link_; 

pfMakeIdentMat(T_matrix); 

} 
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void 

rotaryjink: rotate Jink(float angle) 

{ 

/* joint stops disabled 
if (angle < minjointjingle) 

{ 

rotate( minjointjingle), 
motionjinutflag = -1; 

else if (angle > max joint angle) 

{ 

rotate(ma.\joint_angle); 
motion_limit_flag = 1, 

else 

{ 

*/ 

rotate(angle); 
motion_limit_flag = 0; 

/* 



*/ 

} 

void 

mdhjnatnx (pfMatrix mdh, 

float cosrotate, float sinrotate, 
float costvvist i_l, float sintwist j_l, 
float length_i_l, float translate) 

{ 

/* col 1 *1 

mdh[0|[0] = cosrotate; 
mdh[ 1 ] [0] = - sinrotate; 
mdh(2][0] = O.Of; 
mdh[3][0] = length_i_l; 

/* col 2 V 

mdh[0] [ 1 ] = sinrotate * costvvist _i_l; 
mdh[ 1 ] [ 1 1 = cosrotate * costvvist j_l; 
mdh[2][l] = - sintwist _i_l; 
mdh[3][l] = - sintwist j_ l * translate; 

/* col 3 */ 

mdh[0|[2] = sinrotate * sintwist_i_l; 
mdh [ 1 ] [2] = cosrotate * sintwist_i_l; 
mdh[2]|2] = costwistj_l; 
mdh[3|[2] = costwist_i_ 1 * translate; 

/* col 4 */ 

mdh[0][3] = mdh[ 1 ] [3 ] = mdh[2][3] = O.Of; 
mdh[3][3] = l.Of; 
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/* alternate method using pf functions 
pfVec3 col; 
pfMakeldentMat(mdh); 

pfSetVec3(col, cosrotate, sinrotate * cost\vist_i_l, 
sinrotate * sint\vist_i_l); 
pf5etMatRo\vVec3(mdh, 0, col); 
pfSetVec3(col, -sinrotate, cosrotate * cost\vist_i_l, 
cosrotate * sintvvist_i_l); 
pfSetMatRowVec3(mdh, 1, col); 
pfSetVec3(col, 0.0, -sintvvist_i_l, costvvist_i_l), 
pfSetMatRowVec3(mdh, 2, col). 
pfSetVec3(col, length_i_l, - sint\vist_i_l * translate, 
costvvist_i_l * translate); 
pfSetMatRovvVec3(mdh, 3, col); 

*/ 

I 

/* AQUA-LINK CLASSES */ 

aqualinkO::aqualinkO() 

{ 

maxjoint_angle = deg_to_rad(360.0f); 



aqual ink 1 : :aqualink 1 () 

{ 

iength_i_l = LINKOLENGTH; 
minJoint_angle = JOINT 1 MIN; 
maxjointangle = JOINT 1 MAX; 



aqual i nk2 : : aquali nk2() 

{ 

length_i_l = LINK 1 LENGTH; 
tvvist_i_l = deg_to_rad(-90.0f); 
minjoint_angle = JOINT2MIN; 
maxJoint_angle = JOINT2MAX; 

} 



aqualink3 : :aqualink3() 

{ 

length_i_l = LINK2LENGTH; 
minj'oint_angle = JOINT3MIN; 
maxJoint_angle = JOINT3MAX; 

} 
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aqualink4 aqualink4() 

i 

\ 

Icngth_i_l = L1NK3LENGTH, 
minjointangle = J01NT4MIN; 
maxjomt angle = JOINT4MAX, 

> 
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//file "rigid_body.H" 



#ifndef _RIGID_BODY_ 

^define JUGID_BODY_ 

^include <Performer/pf.h> 

^define GRAVITY 980.0 

class ngid_body 

{ 

public: 

pfVec3 location; /* The vector (x y z) in world coordinates. */ 

pfVec3 vel_trans; /* The vector (u v w) in body coordinates. */ 

pfVec3 vel_rot; /* The vector (p q r) in body coordinates. */ 

pfVec3 accel_trans;/* The vector (u-dot v-dot w-dot). */ 

pfVec3 accel_rot; /* The vector (p-dot q-dot r-dot). */ 

pfVec3 forces; /* The vector (Fx Fy Fz) in body coordinates. */ 

pfVec3 torques; /* The vector (L M N) in body coordinates. */ 

pfVec3 moments_; /* The vector (lx Iy Iz) in principal axis coordinates. */ 

float mass_, /* in Kg. */ 

float current_time; 

pfMatnx H_matrix; 

public: 

rigid_body(float mass, pfVec3 moments = NULL); 
void 

move(float azimuth, float elevation, float roll, 
float x, float y, float z); 

float 

get_delta_t(); 

void 

start_timer(); 



I* 

void 

update_rigid_body(); 

*/ 



void 

update_acceleration(); 

void 

update_velocity(float dt); 
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void 

update_H_matnx(float dt), 
void 

update j3osition(float dt). 
void 

vvorld_to_body(pfVec3 destination. pfVec3 source), 
void 

body_to_\vorld(pfVec3 destination. pfVec3 source), 
void 

add_force_and torques(pfVec3 r, pf\ r ec3 f). 

}: 

void 

homogeneous_transform{ pfMatrix homo. 

float azimuth, float elevation, float roll, 
float x, float y, float z). 



void 

post_mult(pfVec3 destination, pfMatnx m. pfVec3 source), 
#endif 
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//file M rigid_body.C" 



#include M ngid_body.H M 
#include "misc.H" 

rigidbody: rigid J)ody( float mass, pfVec3 moments) 

{ 

location [0] = location [1] - location [2] = 
vel_trans [0] = veltrans [1] = vel_trans [2] = 
vel_rot [0] = vel_rot [1] = vel_rot [2] = 
accel_trans[0] = accel_trans[l ] = accel_trans[2] = 
accel_rot [0] = accel_rot [ 1 ] = accel rot [2] = 
forces [0] = forces [1] = forces [2] = 
torques [0] = torques [ 1 ] = torques [2]=0.0f; 
if (moments == NULL) 

momentsJO] = moments_[ 1 ] = moments_[2] = O.Of; 
else 

pfCopy Vec3 ( moments_. mome nts) ; 
mass_ = mass; 
pfMakeIdentMat(H_matrix); 
current_time = O.Of; 

} 

void 

rigid_body::move(float azimuth, float elevation, float roll, 
float x, float y, float z) 

/ 

i 

homogeneous_transform(H_matrix, azimuth, elevation, roll, x, y, z); 
pfSetVec3 (location, x, y, z); 

} 

float 

rigid_bodv ; : ge t_de lta_t() 

{ 

float dt = 0.05f; 
current_time += dt; 
return dt; 

} 

void 

ri gid_body : : start_timer() 

{ 

currentjjme = O.Of; 

} 
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/* 

void 

rigid_body : update j*igid_body() 

{ 

float dt = getdeltaj; 
update_H_matn.\(dt); 
update_position(dt), 
update_velocit>(dt). 
update __acccleration( ); 

} 

*/ 



void 

rigid Jjody update_acceleration( ) 

/ 

\ 

accel_trans(0] = vel_trans|l] * vel_rot[2| - vcl_trans[2| * vel_rot[l| 

+ forces|0| / mass_ + GRAVITY * H_matnx[0|[2|; 
acceljrans|l| = vcljrans|2| * \ el _rot[()| - vel trans[0| * vel_rot[2| 

+ forccsj 1 1 / mass_ +* GRAVITY * Hmatrix) l |[2j; 
accel_trans[2| = vel _trans[0| * vel_rot[ l ] - vel_trans[ 1 1 * vel_rot|0| 

+ forces[2] / mass_ + GRAVITY * H_matrix[2|[2); 
accel_rot[0] = 

((moments_[ 1| - moments_(2|) * \ el rot| 1 ] * vel rot [2 1 + torques[0]) 
/ monients_[0], 

accel_rot[l| = 

((moments_[2] - moments_[0|) * vel_rot[2| * vel_rot[0] + torques) 1]) 
/ moments JU; 

accel_rot[2| = 

((moments JO] - moments Jl|) * vel_rot[0] * \ el rot) 1 J + torques[2|) 
/ moments_[2|; 



void 

rigid Jx)dy : :update_ve loci tv (float dt) 

{ 

pfVec3 dv; 

pfScaleVec3(dv, dt, acceljrans); 
pfAddVec3(veljrans, vel_trans, dv); 
pfScaleVec3(dv, dt, accel rot); 
pfAddVec3(vel_rot, vel_rot, dv); 

} 
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void 

rigidJ)ody: update Jd_matrix(float dt) 

i 

i 

pfMatnx homo; 

homogeneous jransform( homo, dt * vel_rot[2], 
dt * vel_rot[l], 
dt * vel_rot[0], 
dt * vel_trans[0], 
dt * vel_trans[l], 
dt * vel_trans(2]), 
pfPreMultiMat(H_matrix, homo); 



void 

rigid_body update_position(float dt) 

{ 

pfGetMatRo\vVec3(H_matnx, 3, location); 



void 

rigid_bodv::\vorld_to_body(pfVec3 destination, pfVec3 source) 

{ 

pfMatrix inv_H, 
pflnvertMat(inv_H, Hmatrix); 
post_mult(destination, inv_H, source); 

} 

void 

rigid_bodv::bodv_to_\vorld(pfVec3 destination, pfVec3 source) 

{ 

post_mult(destination, H matrix, source); 

} 

void 

rigid_bodv::add_force_and_torques(pfVec3 r, pfVec3 0 

{ 

pfVec3 t; 

pfCrossVec3(t, r, 0; 
pfAddVec3 (torques, torques, t); 
pfAddVec3(forces, forces, f)i 

} 
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void 

homogeneous_transforin(pfMatnx homo. 

float azimuth, float elevation, float roll, 
float x, float v, float z) 

( 

\ 

float sz, cz, sy, cv, sx, cx; 

pfSinCos(rad jo deg(azimuth), &sz. &cz). 
pfSinCos(radjo_deg(elevation). &sy. &cv); 
pfSinCos(rad_to_deg(roll). &sx, &cx), 

/* col 1 */ 

homo[0][0] = cz*cy, 
homo[ 1 ] [0] = cz*s\ *sx - sz*cx. 
homo[2)[0] = cz*s\*cx + sz*sx, 
homo[3|[0] = x; 

/* col 2 */ 

homo[0][ 1 ) = sz*c\; 
homo[l][l j = cz*cx + sz*sy*sx , 
homo(2][l] = sz*sy*cx + cz*sx; 
homo[3][ 1 1 = y: 

/* col 3 */ 
homo[0][2| =-sy; 
homo] 1 1 [2 1 - cy*sx; 
homo[2][2] = cy*cx; 
homo[3][2] = z; 

/* col 4 V 

homo[()][3] = homo[l)[3] = homo(2][3] = O.Of; 
homo[3][3] = l.Of; 

} 

void 

post_mult(pfVec3 destination, pfMatrix m, pfVec3 source) 

{ 

destination^] = source[0] * m[0][0] + source! 1| * m[ 1 ] [0] + 
source[2] * m[2][0] + m[3][0]; 

destination! 1] = source[0] * m[0][ 1 ] + source! 1] * m[ 1 ][ 1 ] + 
source [2] * m(2][l] + m[3][l]; 

destination[2] = source[0] * m[0][2] + source] 1] * m[ 1 ][2] + 
source[2] * m[2][2] + m[3 ] [2]; 

} 
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//file "misc.H" 



# i fndef _MY_MI S C_ 

^define _MY_MISC_ 

/* t\pe BOOLEAN */ 

typedef int boolean; 

#ifndef TRUE 
#define TRUE 1 
^endif 

#ifndef FALSE 
#define FALSE 0 
#endif 

#definePI 3 14 159265358979323846 
rfdeFine PI_F 3 14159f 

/* angle measurement conversions */ 

float 

deg_to_rad(float deg); 
float 

rad_t o _deg(float rad); 
double 

deg_to_rad(double deg); 
double 

radJ°jieg(double rad); 



#endif 



//file "misc.C” 



#include "nusc.H" 

/* angle measurement conversions */ 
float 

deg _to_rad( float deg) 

\ 

const float rad_per_deg = PI_F 180. Of; 
return (deg * rad_per_deg); 

f 

float 

rad_to_deg(float rad) 

i 

const float deg_per_rad = 1 80. Of / PI_F; 
return (rad * deg_per_rad). 



double 

deg_to_rad(double deg) 

{ 

const double rad_per_deg = PI / 180.0, 
return (deg * rad_per_deg); 

} 

double 

rad_t o _deg(double rad) 

{ 

const double deg_per_rad = 180.0 / PI; 
return (rad * deg_per_rad); 

} 
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